pax_global_header00006660000000000000000000000064127435657100014524gustar00rootroot0000000000000052 comment=7075caf32ddcd5825ff67303902e3db7664a407a fcl-0.5.0/000077500000000000000000000000001274356571000122725ustar00rootroot00000000000000fcl-0.5.0/.appveyor.yml000066400000000000000000000025101274356571000147360ustar00rootroot00000000000000# AppVeyor file # http://www.appveyor.com/docs/appveyor-yml version: "{build}" os: Visual Studio 2015 clone_folder: C:\projects\fcl shallow_clone: true # branches: # only: # - master platform: x64 environment: CTEST_OUTPUT_ON_FAILURE: 1 BOOST_ROOT: C:\Libraries\boost_1_59_0 BOOST_LIBRARYDIR: C:\Libraries\boost_1_59_0\lib64-msvc-14.0 cache: - C:\Program Files\libccd configuration: - Debug - Release before_build: - cmd: if not exist C:\"Program Files"\libccd\lib\ccd.lib ( curl -L -o libccd-2.0.tar.gz https://github.com/danfis/libccd/archive/v2.0.tar.gz && cmake -E tar zxf libccd-2.0.tar.gz && cd libccd-2.0 && cmake -G "Visual Studio 14 2015 Win64" -DCMAKE_BUILD_TYPE=%Configuration% . && cmake --build . --target install --config %Configuration% && cd .. ) else (echo Using cached libccd) - cmd: set - cmd: mkdir build - cmd: cd build - cmd: cmake -G "Visual Studio 14 2015 Win64" -DCMAKE_BUILD_TYPE=%Configuration% -DBOOST_ROOT="%BOOST_ROOT%" -DBOOST_LIBRARYDIR="%BOOST_LIBRARYDIR%" -DCCD_INCLUDE_DIRS="C:\Program Files\libccd\include" -DCCD_LIBRARY="C:\Program Files\libccd\lib\ccd.lib" .. build: project: C:\projects\fcl\build\fcl.sln parallel: true # tests seem to hang # test_script: # - cmd: ctest -C %Configuration% fcl-0.5.0/.travis.yml000066400000000000000000000016041274356571000144040ustar00rootroot00000000000000sudo: required dist: trusty cache: apt: true language: cpp os: - linux - osx compiler: - gcc - clang env: - BUILD_TYPE=Debug COVERALLS=ON - BUILD_TYPE=Release COVERALLS=OFF matrix: exclude: - os: osx compiler: gcc addons: apt: packages: - libboost-all-dev install: # Install dependencies for FCL - if [ "$TRAVIS_OS_NAME" = "linux" ]; then 'ci/install_linux.sh' ; fi - if [ "$TRAVIS_OS_NAME" = "osx" ]; then 'ci/install_osx.sh' ; fi script: # Create build directory - mkdir build - cd build # Configure - cmake -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DFCL_COVERALLS=$COVERALLS .. # Build - make -j4 - if [ $COVERALLS = ON ] && [ "$TRAVIS_OS_NAME" = "linux" ]; then make coveralls; fi # Run unit tests - make test # Make sure we can install and uninstall with no issues - sudo make -j4 install - sudo make -j4 uninstall fcl-0.5.0/CHANGELOG.md000066400000000000000000000045041274356571000141060ustar00rootroot00000000000000## FCL 0 ### FCL 0.5.0 (2016-XX-XX) * Added safe-guards to allow octree headers only if octomap enabled: [#136](https://github.com/flexible-collision-library/fcl/pull/136) * Added CMake option to disable octomap in build: [#135](https://github.com/flexible-collision-library/fcl/pull/135) * Added automatic coverage test reporting: [#125](https://github.com/flexible-collision-library/fcl/pull/125), [#98](https://github.com/flexible-collision-library/fcl/pull/98) * Added CMake exported targets: [#116](https://github.com/flexible-collision-library/fcl/pull/116) * Fixed API to support Octomap 1.8: [#129](https://github.com/flexible-collision-library/fcl/pull/129), [#126](https://github.com/flexible-collision-library/fcl/issues/126) * Fixed continuousCollisionNaive() wasn't resetting the returned result when no collision: [#123](https://github.com/flexible-collision-library/fcl/pull/123) * Fixed uninitialized tf in TranslationMotion: [#121](https://github.com/flexible-collision-library/fcl/pull/121) * Fixed fcl.pc populated incorrect installation paths: [#118](https://github.com/flexible-collision-library/fcl/pull/118) * Fixed octree vs mesh CollisionResult now returns triangle id: [#114](https://github.com/flexible-collision-library/fcl/pull/114) * Fixed minor typo: [#113](https://github.com/flexible-collision-library/fcl/pull/113) * Fixed fallback finding of libccd: [#112](https://github.com/flexible-collision-library/fcl/pull/112) * Fixed a nasty bug in propagate propagateBVHFrontListCollisionRecurse(): [#110](https://github.com/flexible-collision-library/fcl/pull/110) * Fixed test_fcl_math failures on Windows 64 bit due to non-portable use of long: [#108](https://github.com/flexible-collision-library/fcl/pull/108), [#107](https://github.com/flexible-collision-library/fcl/issues/107) * Fixed compilation in Visual Studio 2015, and suppressed some warnings: [#99](https://github.com/flexible-collision-library/fcl/pull/99) * Fixed build when libccd package config not found: [#94](https://github.com/flexible-collision-library/fcl/pull/94) * Removing dependency on boost: [#108](https://github.com/flexible-collision-library/fcl/pull/108), [#105](https://github.com/flexible-collision-library/fcl/pull/105), [#104](https://github.com/flexible-collision-library/fcl/pull/104), [#103](https://github.com/flexible-collision-library/fcl/pull/103) fcl-0.5.0/CMakeLists.txt000066400000000000000000000126241274356571000150370ustar00rootroot00000000000000cmake_minimum_required(VERSION 2.8.12) # Use MACOSX_RPATH by default on OS X. This was added in CMake 2.8.12 and # became default in CMake 3.0. Explicitly setting this policy is necessary to # suppress a warning in CMake 3.0 and above. if(POLICY CMP0042) cmake_policy(SET CMP0042 NEW) endif() project(fcl CXX C) # set the default build type if (NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) endif() # Set build type variable set(FCL_BUILD_TYPE_RELEASE FALSE) set(FCL_BUILD_TYPE_DEBUG FALSE) string(TOUPPER ${CMAKE_BUILD_TYPE} CMAKE_BUILD_TYPE_UPPERCASE) if("${CMAKE_BUILD_TYPE_UPPERCASE}" STREQUAL "RELEASE") set(FCL_BUILD_TYPE_RELEASE TRUE) elseif("${CMAKE_BUILD_TYPE_UPPERCASE}" STREQUAL "DEBUG") set(FCL_BUILD_TYPE_DEBUG TRUE) else() message(STATUS "CMAKE_BUILD_TYPE ${CMAKE_BUILD_TYPE} unknown. Valid options are: Debug Release") endif() # This shouldn't be necessary, but there has been trouble # with MSVC being set off, but MSVCXX ON. if(MSVC OR MSVC90 OR MSVC10) set(MSVC ON) endif (MSVC OR MSVC90 OR MSVC10) set(LIBRARY_OUTPUT_PATH ${PROJECT_BINARY_DIR}/lib) set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules") include(CompilerSettings) include(FCLVersion) include(GNUInstallDirs) if(MSVC OR IS_ICPC) option(FCL_STATIC_LIBRARY "Whether the FCL library should be static rather than shared" ON) else() option(FCL_STATIC_LIBRARY "Whether the FCL library should be static rather than shared" OFF) endif() # Whether to enable SSE option(FCL_USE_SSE "Whether FCL should SSE instructions" OFF) set(FCL_HAVE_SSE 0) if(FCL_USE_SSE) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID STREQUAL "Clang") set(FCL_HAVE_SSE 0) #always disable, for now add_definitions(-march=native) endif() # TODO: do something similar for other compilers endif() # Coveralls support option(FCL_COVERALLS "Turn on coveralls support" OFF) option(FCL_COVERALLS_UPLOAD "Upload the generated coveralls json" ON) if(FCL_COVERALLS) include(Coveralls) coveralls_turn_on_coverage() endif() # Find Octomap (optional) find_package(PkgConfig QUIET) option(FCL_WITH_OCTOMAP "octomap library support" ON) set(FCL_HAVE_OCTOMAP 0) if(FCL_WITH_OCTOMAP) if(PKG_CONFIG_FOUND) pkg_check_modules(OCTOMAP QUIET octomap) endif() if(NOT OCTOMAP_FOUND) # if pkgconfig is not installed, then fall back on more fragile detection # of octomap find_path(OCTOMAP_INCLUDE_DIRS octomap.h PATH_SUFFIXES octomap) find_library(OCTOMAP_LIBRARY_DIRS ${CMAKE_SHARED_LIBRARY_PREFIX}octomap${CMAKE_SHARED_LIBRARY_SUFFIX}) if(OCTOMAP_INCLUDE_DIRS AND OCTOMAP_LIBRARY_DIRS) set(OCTOMAP_LIBRARIES "octomap;octomath") endif() endif() if (OCTOMAP_FOUND OR (OCTOMAP_INCLUDE_DIRS AND OCTOMAP_LIBRARY_DIRS)) string(REPLACE "." ";" VERSION_LIST ${OCTOMAP_VERSION}) list(GET VERSION_LIST 0 OCTOMAP_MAJOR_VERSION) list(GET VERSION_LIST 1 OCTOMAP_MINOR_VERSION) list(GET VERSION_LIST 2 OCTOMAP_PATCH_VERSION) include_directories(${OCTOMAP_INCLUDE_DIRS}) link_directories(${OCTOMAP_LIBRARY_DIRS}) set(FCL_HAVE_OCTOMAP 1) message(STATUS "FCL uses Octomap") else() message(STATUS "FCL does not use Octomap") endif() else() message(STATUS "FCL does not use Octomap (as requested)") endif() # find_package(tinyxml QUIET) # if (TINYXML_FOUND) # set(FCL_HAVE_TINYXML 1) # include_directories(${TINYXML_INCLUDE_DIRS}) # link_directories(${TINYXML_LIBRARY_DIRS}) # message(STATUS "FCL uses tinyxml") # else() # message(STATUS "FCL does not use tinyxml") # endif() # FCL's own include dir should be at the front of the include path include_directories(BEFORE "include") include_directories("${CMAKE_CURRENT_BINARY_DIR}/include") if(PKG_CONFIG_FOUND) pkg_check_modules(CCD ccd) # check to see if the pkg is installed under the libccd name if(NOT CCD_FOUND) pkg_check_modules(CCD libccd) endif() endif() if(NOT CCD_FOUND) # if pkgconfig is not installed, then fall back on more fragile detection # of ccd find_path(CCD_INCLUDE_DIRS ccd/ccd.h) find_library(CCD_LIBRARY ${CMAKE_SHARED_LIBRARY_PREFIX}ccd${CMAKE_SHARED_LIBRARY_SUFFIX}) if(CCD_INCLUDE_DIRS AND CCD_LIBRARY) set(CCD_LIBRARIES "${CCD_LIBRARY}") else() message(FATAL_ERROR "Libccd is required by FCL") endif() endif() include_directories(${CCD_INCLUDE_DIRS}) link_directories(${CCD_LIBRARY_DIRS}) add_subdirectory(include/fcl) add_subdirectory(src) set(pkg_conf_file_in "${CMAKE_CURRENT_SOURCE_DIR}/fcl.pc.in") set(pkg_conf_file_out "${CMAKE_CURRENT_BINARY_DIR}/fcl.pc") if(NOT MSVC) set(PKG_CFLAGS "-std=c++11") endif() configure_file("${pkg_conf_file_in}" "${pkg_conf_file_out}" @ONLY) install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} FILES_MATCHING PATTERN "*.h" PATTERN "*.hxx" PATTERN ".DS_Store" EXCLUDE ) install(FILES "${pkg_conf_file_out}" DESTINATION ${CMAKE_INSTALL_LIBDIR}/pkgconfig/ COMPONENT pkgconfig) # Add uninstall target configure_file( "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules/cmake_uninstall.cmake.in" "${CMAKE_CURRENT_BINARY_DIR}/CMakeModules/cmake_uninstall.cmake" IMMEDIATE @ONLY) add_custom_target(uninstall "${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/CMakeModules/cmake_uninstall.cmake") find_package(Boost COMPONENTS date_time filesystem system unit_test_framework) option(FCL_BUILD_TESTS "Build FCL tests" ${Boost_FOUND}) if(FCL_BUILD_TESTS) enable_testing() add_subdirectory(test) endif() fcl-0.5.0/CMakeModules/000077500000000000000000000000001274356571000146035ustar00rootroot00000000000000fcl-0.5.0/CMakeModules/CompilerSettings.cmake000066400000000000000000000037301274356571000211030ustar00rootroot00000000000000# force C++11 mode add_definitions() if(CMAKE_COMPILER_IS_GNUCXX) add_definitions(-std=c++11 -W -Wall -g -Wextra -Wno-missing-field-initializers -Wno-unused-parameter) endif(CMAKE_COMPILER_IS_GNUCXX) if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") add_definitions(-std=c++11 -W -Wall -Wextra -Wno-missing-field-initializers -Wno-unused-parameter -Wno-delete-non-virtual-dtor -Wno-overloaded-virtual -Wno-unknown-pragmas -Wno-deprecated-register) endif() if(MSVC OR MSVC90 OR MSVC10) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /EHsc /MP /W1") endif(MSVC OR MSVC90 OR MSVC10) if(CMAKE_CXX_COMPILER_ID STREQUAL "Intel") set(IS_ICPC 1) else() set(IS_ICPC 0) endif() if(IS_ICPC) add_definitions(-std=c++11 -wd191 -wd411 -wd654 -wd1125 -wd1292 -wd1565 -wd1628 -wd2196) set(CMAKE_AR "xiar" CACHE STRING "Intel archiver" FORCE) set(CMAKE_CXX_FLAGS "-pthread" CACHE STRING "Default compile flags" FORCE) set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG" CACHE STRING "Flags used by the C++ compiler during release builds." FORCE) set(CMAKE_CXX_FLAGS_DEBUG "-O0 -g" CACHE STRING "Flags used by the C++ compiler during debug builds." FORCE) set(CMAKE_LINKER "xild" CACHE STRING "Intel linker" FORCE) endif(IS_ICPC) if(CMAKE_CXX_COMPILER_ID STREQUAL "XL") set(IS_XLC 1) else() set(IS_XLC 0) endif() if(IS_XLC) add_definitions(-std=c++11 -qpic -q64 -qmaxmem=-1) set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -q64") set(CMAKE_MODULE_LINKER_FLAGS "${CMAKE_MODULE_LINKER_FLAGS} -q64") endif(IS_XLC) if((CMAKE_COMPILER_IS_GNUCXX OR IS_ICPC) AND NOT MINGW) add_definitions(-fPIC) endif((CMAKE_COMPILER_IS_GNUCXX OR IS_ICPC) AND NOT MINGW) # Set rpath http://www.paraview.org/Wiki/CMake_RPATH_handling set(CMAKE_SKIP_BUILD_RPATH FALSE) set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE) set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_LIBDIR_FULL}") set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) # no prefix needed for python modules set(CMAKE_SHARED_MODULE_PREFIX "") fcl-0.5.0/CMakeModules/Coveralls.cmake000066400000000000000000000115411274356571000175410ustar00rootroot00000000000000# # The MIT License (MIT) # # 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. # # Copyright (C) 2014 Joakim Söderberg # set(_CMAKE_SCRIPT_PATH ${CMAKE_CURRENT_LIST_DIR}) # must be outside coveralls_setup() to get correct path # # Param _COVERAGE_SRCS A list of source files that coverage should be collected for. # Param _COVERALLS_UPLOAD Upload the result to coveralls? # function(coveralls_setup _COVERAGE_SRCS _COVERALLS_UPLOAD) if (ARGC GREATER 2) set(_CMAKE_SCRIPT_PATH ${ARGN}) message(STATUS "Coveralls: Using alternate CMake script dir: ${_CMAKE_SCRIPT_PATH}") endif() if (NOT EXISTS "${_CMAKE_SCRIPT_PATH}/CoverallsClear.cmake") message(FATAL_ERROR "Coveralls: Missing ${_CMAKE_SCRIPT_PATH}/CoverallsClear.cmake") endif() if (NOT EXISTS "${_CMAKE_SCRIPT_PATH}/CoverallsGenerateGcov.cmake") message(FATAL_ERROR "Coveralls: Missing ${_CMAKE_SCRIPT_PATH}/CoverallsGenerateGcov.cmake") endif() # When passing a CMake list to an external process, the list # will be converted from the format "1;2;3" to "1 2 3". # This means the script we're calling won't see it as a list # of sources, but rather just one long path. We remedy this # by replacing ";" with "*" and then reversing that in the script # that we're calling. # http://cmake.3232098.n2.nabble.com/Passing-a-CMake-list-quot-as-is-quot-to-a-custom-target-td6505681.html set(COVERAGE_SRCS_TMP ${_COVERAGE_SRCS}) set(COVERAGE_SRCS "") foreach (COVERAGE_SRC ${COVERAGE_SRCS_TMP}) set(COVERAGE_SRCS "${COVERAGE_SRCS}*${COVERAGE_SRC}") endforeach() #message("Coverage sources: ${COVERAGE_SRCS}") set(COVERALLS_FILE ${PROJECT_BINARY_DIR}/coveralls.json) add_custom_target(coveralls_generate # Zero the coverage counters. COMMAND ${CMAKE_COMMAND} -DPROJECT_BINARY_DIR="${PROJECT_BINARY_DIR}" -P "${_CMAKE_SCRIPT_PATH}/CoverallsClear.cmake" # Run regress tests. COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure # Generate Gcov and translate it into coveralls JSON. # We do this by executing an external CMake script. # (We don't want this to run at CMake generation time, but after compilation and everything has run). COMMAND ${CMAKE_COMMAND} -DCOVERAGE_SRCS="${COVERAGE_SRCS}" # TODO: This is passed like: "a b c", not "a;b;c" -DCOVERALLS_OUTPUT_FILE="${COVERALLS_FILE}" -DCOV_PATH="${PROJECT_BINARY_DIR}" -DPROJECT_ROOT="${PROJECT_SOURCE_DIR}" -P "${_CMAKE_SCRIPT_PATH}/CoverallsGenerateGcov.cmake" WORKING_DIRECTORY ${PROJECT_BINARY_DIR} COMMENT "Generating coveralls output..." ) if (_COVERALLS_UPLOAD) message("COVERALLS UPLOAD: ON") find_program(CURL_EXECUTABLE curl) if (NOT CURL_EXECUTABLE) message(FATAL_ERROR "Coveralls: curl not found! Aborting") endif() add_custom_target(coveralls_upload # Upload the JSON to coveralls. COMMAND ${CURL_EXECUTABLE} -S -F json_file=@${COVERALLS_FILE} https://coveralls.io/api/v1/jobs DEPENDS coveralls_generate WORKING_DIRECTORY ${PROJECT_BINARY_DIR} COMMENT "Uploading coveralls output...") add_custom_target(coveralls DEPENDS coveralls_upload) else() message("COVERALLS UPLOAD: OFF") add_custom_target(coveralls DEPENDS coveralls_generate) endif() endfunction() macro(coveralls_turn_on_coverage) if(NOT (CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX) AND (NOT "${CMAKE_C_COMPILER_ID}" STREQUAL "Clang")) message(FATAL_ERROR "Coveralls: Compiler ${CMAKE_C_COMPILER_ID} is not GNU gcc! Aborting... You can set this on the command line using CC=/usr/bin/gcc CXX=/usr/bin/g++ cmake ..") endif() if(NOT CMAKE_BUILD_TYPE STREQUAL "Debug") message(FATAL_ERROR "Coveralls: Code coverage results with an optimised (non-Debug) build may be misleading! Add -DCMAKE_BUILD_TYPE=Debug") endif() set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -O0 -fprofile-arcs -ftest-coverage") set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -g -O0 -fprofile-arcs -ftest-coverage") endmacro() fcl-0.5.0/CMakeModules/CoverallsClear.cmake000066400000000000000000000025271274356571000205140ustar00rootroot00000000000000# # The MIT License (MIT) # # 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. # # Copyright (C) 2014 Joakim Söderberg # # do not follow symlinks in file(GLOB_RECURSE ...) cmake_policy(SET CMP0009 NEW) file(GLOB_RECURSE GCDA_FILES "${PROJECT_BINARY_DIR}/*.gcda") if(NOT GCDA_FILES STREQUAL "") file(REMOVE ${GCDA_FILES}) endif() fcl-0.5.0/CMakeModules/CoverallsGenerateGcov.cmake000066400000000000000000000407751274356571000220460ustar00rootroot00000000000000# # The MIT License (MIT) # # 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. # # Copyright (C) 2014 Joakim Söderberg # # This is intended to be run by a custom target in a CMake project like this. # 0. Compile program with coverage support. # 1. Clear coverage data. (Recursively delete *.gcda in build dir) # 2. Run the unit tests. # 3. Run this script specifying which source files the coverage should be performed on. # # This script will then use gcov to generate .gcov files in the directory specified # via the COV_PATH var. This should probably be the same as your cmake build dir. # # It then parses the .gcov files to convert them into the Coveralls JSON format: # https://coveralls.io/docs/api # # Example for running as standalone CMake script from the command line: # (Note it is important the -P is at the end...) # $ cmake -DCOV_PATH=$(pwd) # -DCOVERAGE_SRCS="catcierge_rfid.c;catcierge_timer.c" # -P ../cmake/CoverallsGcovUpload.cmake # CMAKE_MINIMUM_REQUIRED(VERSION 2.8) # # Make sure we have the needed arguments. # if (NOT COVERALLS_OUTPUT_FILE) message(FATAL_ERROR "Coveralls: No coveralls output file specified. Please set COVERALLS_OUTPUT_FILE") endif() if (NOT COV_PATH) message(FATAL_ERROR "Coveralls: Missing coverage directory path where gcov files will be generated. Please set COV_PATH") endif() if (NOT COVERAGE_SRCS) message(FATAL_ERROR "Coveralls: Missing the list of source files that we should get the coverage data for COVERAGE_SRCS") endif() if (NOT PROJECT_ROOT) message(FATAL_ERROR "Coveralls: Missing PROJECT_ROOT.") endif() # Since it's not possible to pass a CMake list properly in the # "1;2;3" format to an external process, we have replaced the # ";" with "*", so reverse that here so we get it back into the # CMake list format. string(REGEX REPLACE "\\*" ";" COVERAGE_SRCS ${COVERAGE_SRCS}) if (NOT DEFINED ENV{GCOV}) find_program(GCOV_EXECUTABLE gcov) else() find_program(GCOV_EXECUTABLE $ENV{GCOV}) endif() # convert all paths in COVERAGE_SRCS to absolute paths set(COVERAGE_SRCS_TMP "") foreach (COVERAGE_SRC ${COVERAGE_SRCS}) if (NOT "${COVERAGE_SRC}" MATCHES "^/") set(COVERAGE_SRC ${PROJECT_ROOT}/${COVERAGE_SRC}) endif() list(APPEND COVERAGE_SRCS_TMP ${COVERAGE_SRC}) endforeach() set(COVERAGE_SRCS ${COVERAGE_SRCS_TMP}) unset(COVERAGE_SRCS_TMP) if (NOT GCOV_EXECUTABLE) message(FATAL_ERROR "gcov not found! Aborting...") endif() find_package(Git) set(JSON_REPO_TEMPLATE "{ \"head\": { \"id\": \"\@GIT_COMMIT_HASH\@\", \"author_name\": \"\@GIT_AUTHOR_NAME\@\", \"author_email\": \"\@GIT_AUTHOR_EMAIL\@\", \"committer_name\": \"\@GIT_COMMITTER_NAME\@\", \"committer_email\": \"\@GIT_COMMITTER_EMAIL\@\", \"message\": \"\@GIT_COMMIT_MESSAGE\@\" }, \"branch\": \"@GIT_BRANCH@\", \"remotes\": [] }" ) # TODO: Fill in git remote data if (GIT_FOUND) # Branch. execute_process( COMMAND ${GIT_EXECUTABLE} rev-parse --abbrev-ref HEAD WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} OUTPUT_VARIABLE GIT_BRANCH OUTPUT_STRIP_TRAILING_WHITESPACE ) macro (git_log_format FORMAT_CHARS VAR_NAME) execute_process( COMMAND ${GIT_EXECUTABLE} log -1 --pretty=format:%${FORMAT_CHARS} WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} OUTPUT_VARIABLE ${VAR_NAME} OUTPUT_STRIP_TRAILING_WHITESPACE ) endmacro() git_log_format(an GIT_AUTHOR_NAME) git_log_format(ae GIT_AUTHOR_EMAIL) git_log_format(cn GIT_COMMITTER_NAME) git_log_format(ce GIT_COMMITTER_EMAIL) git_log_format(B GIT_COMMIT_MESSAGE) git_log_format(H GIT_COMMIT_HASH) string(REPLACE "\n" "\\n" GIT_COMMIT_MESSAGE ${GIT_COMMIT_MESSAGE}) message("Git exe: ${GIT_EXECUTABLE}") message("Git branch: ${GIT_BRANCH}") message("Git author: ${GIT_AUTHOR_NAME}") message("Git e-mail: ${GIT_AUTHOR_EMAIL}") message("Git commiter name: ${GIT_COMMITTER_NAME}") message("Git commiter e-mail: ${GIT_COMMITTER_EMAIL}") message("Git commit hash: ${GIT_COMMIT_HASH}") message("Git commit message: ${GIT_COMMIT_MESSAGE}") string(CONFIGURE ${JSON_REPO_TEMPLATE} JSON_REPO_DATA) else() set(JSON_REPO_DATA "{}") endif() ############################# Macros ######################################### # # This macro converts from the full path format gcov outputs: # # /path/to/project/root/build/#path#to#project#root#subdir#the_file.c.gcov # # to the original source file path the .gcov is for: # # /path/to/project/root/subdir/the_file.c # macro(get_source_path_from_gcov_filename _SRC_FILENAME _GCOV_FILENAME) # /path/to/project/root/build/#path#to#project#root#subdir#the_file.c.gcov # -> # #path#to#project#root#subdir#the_file.c.gcov get_filename_component(_GCOV_FILENAME_WEXT ${_GCOV_FILENAME} NAME) # #path#to#project#root#subdir#the_file.c.gcov -> /path/to/project/root/subdir/the_file.c string(REGEX REPLACE "\\.gcov$" "" SRC_FILENAME_TMP ${_GCOV_FILENAME_WEXT}) string(REGEX REPLACE "\#" "/" SRC_FILENAME_TMP ${SRC_FILENAME_TMP}) set(${_SRC_FILENAME} "${SRC_FILENAME_TMP}") endmacro() ############################################################################## # Get the coverage data. file(GLOB_RECURSE GCDA_FILES "${COV_PATH}/*.gcda") message("GCDA files:") # Get a list of all the object directories needed by gcov # (The directories the .gcda files and .o files are found in) # and run gcov on those. foreach(GCDA ${GCDA_FILES}) message("Process: ${GCDA}") message("------------------------------------------------------------------------------") get_filename_component(GCDA_DIR ${GCDA} PATH) # # The -p below refers to "Preserve path components", # This means that the generated gcov filename of a source file will # keep the original files entire filepath, but / is replaced with #. # Example: # # /path/to/project/root/build/CMakeFiles/the_file.dir/subdir/the_file.c.gcda # ------------------------------------------------------------------------------ # File '/path/to/project/root/subdir/the_file.c' # Lines executed:68.34% of 199 # /path/to/project/root/subdir/the_file.c:creating '#path#to#project#root#subdir#the_file.c.gcov' # # If -p is not specified then the file is named only "the_file.c.gcov" # execute_process( COMMAND ${GCOV_EXECUTABLE} -p -o ${GCDA_DIR} ${GCDA} WORKING_DIRECTORY ${COV_PATH} ) endforeach() # TODO: Make these be absolute path file(GLOB ALL_GCOV_FILES ${COV_PATH}/*.gcov) # Get only the filenames to use for filtering. #set(COVERAGE_SRCS_NAMES "") #foreach (COVSRC ${COVERAGE_SRCS}) # get_filename_component(COVSRC_NAME ${COVSRC} NAME) # message("${COVSRC} -> ${COVSRC_NAME}") # list(APPEND COVERAGE_SRCS_NAMES "${COVSRC_NAME}") #endforeach() # # Filter out all but the gcov files we want. # # We do this by comparing the list of COVERAGE_SRCS filepaths that the # user wants the coverage data for with the paths of the generated .gcov files, # so that we only keep the relevant gcov files. # # Example: # COVERAGE_SRCS = # /path/to/project/root/subdir/the_file.c # # ALL_GCOV_FILES = # /path/to/project/root/build/#path#to#project#root#subdir#the_file.c.gcov # /path/to/project/root/build/#path#to#project#root#subdir#other_file.c.gcov # # Result should be: # GCOV_FILES = # /path/to/project/root/build/#path#to#project#root#subdir#the_file.c.gcov # set(GCOV_FILES "") #message("Look in coverage sources: ${COVERAGE_SRCS}") message("\nFilter out unwanted GCOV files:") message("===============================") set(COVERAGE_SRCS_REMAINING ${COVERAGE_SRCS}) foreach (GCOV_FILE ${ALL_GCOV_FILES}) # # /path/to/project/root/build/#path#to#project#root#subdir#the_file.c.gcov # -> # /path/to/project/root/subdir/the_file.c get_source_path_from_gcov_filename(GCOV_SRC_PATH ${GCOV_FILE}) file(RELATIVE_PATH GCOV_SRC_REL_PATH "${PROJECT_ROOT}" "${GCOV_SRC_PATH}") # Is this in the list of source files? # TODO: We want to match against relative path filenames from the source file root... list(FIND COVERAGE_SRCS ${GCOV_SRC_PATH} WAS_FOUND) if (NOT WAS_FOUND EQUAL -1) message("YES: ${GCOV_FILE}") list(APPEND GCOV_FILES ${GCOV_FILE}) # We remove it from the list, so we don't bother searching for it again. # Also files left in COVERAGE_SRCS_REMAINING after this loop ends should # have coverage data generated from them (no lines are covered). list(REMOVE_ITEM COVERAGE_SRCS_REMAINING ${GCOV_SRC_PATH}) else() message("NO: ${GCOV_FILE}") endif() endforeach() # TODO: Enable setting these set(JSON_SERVICE_NAME "travis-ci") set(JSON_SERVICE_JOB_ID $ENV{TRAVIS_JOB_ID}) set(JSON_REPO_TOKEN $ENV{COVERALLS_REPO_TOKEN}) set(JSON_TEMPLATE "{ \"repo_token\": \"\@JSON_REPO_TOKEN\@\", \"service_name\": \"\@JSON_SERVICE_NAME\@\", \"service_job_id\": \"\@JSON_SERVICE_JOB_ID\@\", \"source_files\": \@JSON_GCOV_FILES\@, \"git\": \@JSON_REPO_DATA\@ }" ) set(SRC_FILE_TEMPLATE "{ \"name\": \"\@GCOV_SRC_REL_PATH\@\", \"source_digest\": \"\@GCOV_CONTENTS_MD5\@\", \"coverage\": \@GCOV_FILE_COVERAGE\@ }" ) message("\nGenerate JSON for files:") message("=========================") set(JSON_GCOV_FILES "[") # Read the GCOV files line by line and get the coverage data. foreach (GCOV_FILE ${GCOV_FILES}) get_source_path_from_gcov_filename(GCOV_SRC_PATH ${GCOV_FILE}) file(RELATIVE_PATH GCOV_SRC_REL_PATH "${PROJECT_ROOT}" "${GCOV_SRC_PATH}") # The new coveralls API doesn't need the entire source (Yay!) # However, still keeping that part for now. Will cleanup in the future. file(MD5 "${GCOV_SRC_PATH}" GCOV_CONTENTS_MD5) message("MD5: ${GCOV_SRC_PATH} = ${GCOV_CONTENTS_MD5}") # Loads the gcov file as a list of lines. # (We first open the file and replace all occurences of [] with _ # because CMake will fail to parse a line containing unmatched brackets... # also the \ to escaped \n in macros screws up things.) # https://public.kitware.com/Bug/view.php?id=15369 file(READ ${GCOV_FILE} GCOV_CONTENTS) string(REPLACE "[" "_" GCOV_CONTENTS "${GCOV_CONTENTS}") string(REPLACE "]" "_" GCOV_CONTENTS "${GCOV_CONTENTS}") string(REPLACE "\\" "_" GCOV_CONTENTS "${GCOV_CONTENTS}") # Remove file contents to avoid encoding issues (cmake 2.8 has no ENCODING option) string(REGEX REPLACE "([^:]*):([^:]*):([^\n]*)\n" "\\1:\\2: \n" GCOV_CONTENTS "${GCOV_CONTENTS}") file(WRITE ${GCOV_FILE}_tmp "${GCOV_CONTENTS}") file(STRINGS ${GCOV_FILE}_tmp GCOV_LINES) list(LENGTH GCOV_LINES LINE_COUNT) # Instead of trying to parse the source from the # gcov file, simply read the file contents from the source file. # (Parsing it from the gcov is hard because C-code uses ; in many places # which also happens to be the same as the CMake list delimeter). file(READ ${GCOV_SRC_PATH} GCOV_FILE_SOURCE) string(REPLACE "\\" "\\\\" GCOV_FILE_SOURCE "${GCOV_FILE_SOURCE}") string(REGEX REPLACE "\"" "\\\\\"" GCOV_FILE_SOURCE "${GCOV_FILE_SOURCE}") string(REPLACE "\t" "\\\\t" GCOV_FILE_SOURCE "${GCOV_FILE_SOURCE}") string(REPLACE "\r" "\\\\r" GCOV_FILE_SOURCE "${GCOV_FILE_SOURCE}") string(REPLACE "\n" "\\\\n" GCOV_FILE_SOURCE "${GCOV_FILE_SOURCE}") # According to http://json.org/ these should be escaped as well. # Don't know how to do that in CMake however... #string(REPLACE "\b" "\\\\b" GCOV_FILE_SOURCE "${GCOV_FILE_SOURCE}") #string(REPLACE "\f" "\\\\f" GCOV_FILE_SOURCE "${GCOV_FILE_SOURCE}") #string(REGEX REPLACE "\u([a-fA-F0-9]{4})" "\\\\u\\1" GCOV_FILE_SOURCE "${GCOV_FILE_SOURCE}") # We want a json array of coverage data as a single string # start building them from the contents of the .gcov set(GCOV_FILE_COVERAGE "[") set(GCOV_LINE_COUNT 1) # Line number for the .gcov. set(DO_SKIP 0) foreach (GCOV_LINE ${GCOV_LINES}) #message("${GCOV_LINE}") # Example of what we're parsing: # Hitcount |Line | Source # " 8: 26: if (!allowed || (strlen(allowed) == 0))" string(REGEX REPLACE "^([^:]*):([^:]*):(.*)$" "\\1;\\2;\\3" RES "${GCOV_LINE}") # Check if we should exclude lines using the Lcov syntax. string(REGEX MATCH "LCOV_EXCL_START" START_SKIP "${GCOV_LINE}") string(REGEX MATCH "LCOV_EXCL_END" END_SKIP "${GCOV_LINE}") string(REGEX MATCH "LCOV_EXCL_LINE" LINE_SKIP "${GCOV_LINE}") set(RESET_SKIP 0) if (LINE_SKIP AND NOT DO_SKIP) set(DO_SKIP 1) set(RESET_SKIP 1) endif() if (START_SKIP) set(DO_SKIP 1) message("${GCOV_LINE_COUNT}: Start skip") endif() if (END_SKIP) set(DO_SKIP 0) endif() list(LENGTH RES RES_COUNT) if (RES_COUNT GREATER 2) list(GET RES 0 HITCOUNT) list(GET RES 1 LINE) list(GET RES 2 SOURCE) string(STRIP ${HITCOUNT} HITCOUNT) string(STRIP ${LINE} LINE) # Lines with 0 line numbers are metadata and can be ignored. if (NOT ${LINE} EQUAL 0) if (DO_SKIP) set(GCOV_FILE_COVERAGE "${GCOV_FILE_COVERAGE}null, ") else() # Translate the hitcount into valid JSON values. if (${HITCOUNT} STREQUAL "#####" OR ${HITCOUNT} STREQUAL "=====") set(GCOV_FILE_COVERAGE "${GCOV_FILE_COVERAGE}0, ") elseif (${HITCOUNT} STREQUAL "-") set(GCOV_FILE_COVERAGE "${GCOV_FILE_COVERAGE}null, ") else() set(GCOV_FILE_COVERAGE "${GCOV_FILE_COVERAGE}${HITCOUNT}, ") endif() endif() endif() else() message(WARNING "Failed to properly parse line (RES_COUNT = ${RES_COUNT}) ${GCOV_FILE}:${GCOV_LINE_COUNT}\n-->${GCOV_LINE}") endif() if (RESET_SKIP) set(DO_SKIP 0) endif() math(EXPR GCOV_LINE_COUNT "${GCOV_LINE_COUNT}+1") endforeach() message("${GCOV_LINE_COUNT} of ${LINE_COUNT} lines read!") # Advanced way of removing the trailing comma in the JSON array. # "[1, 2, 3, " -> "[1, 2, 3" string(REGEX REPLACE ",[ ]*$" "" GCOV_FILE_COVERAGE ${GCOV_FILE_COVERAGE}) # Append the trailing ] to complete the JSON array. set(GCOV_FILE_COVERAGE "${GCOV_FILE_COVERAGE}]") # Generate the final JSON for this file. message("Generate JSON for file: ${GCOV_SRC_REL_PATH}...") string(CONFIGURE ${SRC_FILE_TEMPLATE} FILE_JSON) set(JSON_GCOV_FILES "${JSON_GCOV_FILES}${FILE_JSON}, ") endforeach() # Loop through all files we couldn't find any coverage for # as well, and generate JSON for those as well with 0% coverage. foreach(NOT_COVERED_SRC ${COVERAGE_SRCS_REMAINING}) # Set variables for json replacement set(GCOV_SRC_PATH ${NOT_COVERED_SRC}) file(MD5 "${GCOV_SRC_PATH}" GCOV_CONTENTS_MD5) file(RELATIVE_PATH GCOV_SRC_REL_PATH "${PROJECT_ROOT}" "${GCOV_SRC_PATH}") # Loads the source file as a list of lines. file(STRINGS ${NOT_COVERED_SRC} SRC_LINES) set(GCOV_FILE_COVERAGE "[") set(GCOV_FILE_SOURCE "") foreach (SOURCE ${SRC_LINES}) set(GCOV_FILE_COVERAGE "${GCOV_FILE_COVERAGE}null, ") string(REPLACE "\\" "\\\\" SOURCE "${SOURCE}") string(REGEX REPLACE "\"" "\\\\\"" SOURCE "${SOURCE}") string(REPLACE "\t" "\\\\t" SOURCE "${SOURCE}") string(REPLACE "\r" "\\\\r" SOURCE "${SOURCE}") set(GCOV_FILE_SOURCE "${GCOV_FILE_SOURCE}${SOURCE}\\n") endforeach() # Remove trailing comma, and complete JSON array with ] string(REGEX REPLACE ",[ ]*$" "" GCOV_FILE_COVERAGE ${GCOV_FILE_COVERAGE}) set(GCOV_FILE_COVERAGE "${GCOV_FILE_COVERAGE}]") # Generate the final JSON for this file. message("Generate JSON for non-gcov file: ${NOT_COVERED_SRC}...") string(CONFIGURE ${SRC_FILE_TEMPLATE} FILE_JSON) set(JSON_GCOV_FILES "${JSON_GCOV_FILES}${FILE_JSON}, ") endforeach() # Get rid of trailing comma. string(REGEX REPLACE ",[ ]*$" "" JSON_GCOV_FILES ${JSON_GCOV_FILES}) set(JSON_GCOV_FILES "${JSON_GCOV_FILES}]") # Generate the final complete JSON! message("Generate final JSON...") string(CONFIGURE ${JSON_TEMPLATE} JSON) file(WRITE "${COVERALLS_OUTPUT_FILE}" "${JSON}") message("###########################################################################") message("Generated coveralls JSON containing coverage data:") message("${COVERALLS_OUTPUT_FILE}") message("###########################################################################") fcl-0.5.0/CMakeModules/FCLVersion.cmake000066400000000000000000000004101274356571000175520ustar00rootroot00000000000000# set the version in a way CMake can use set(FCL_MAJOR_VERSION 0) set(FCL_MINOR_VERSION 5) set(FCL_PATCH_VERSION 0) set(FCL_VERSION "${FCL_MAJOR_VERSION}.${FCL_MINOR_VERSION}.${FCL_PATCH_VERSION}") # increment this when we have ABI changes set(FCL_ABI_VERSION 7) fcl-0.5.0/CMakeModules/Findtinyxml.cmake000066400000000000000000000013141274356571000201110ustar00rootroot00000000000000# this module was taken from http://trac.evemu.org/browser/trunk/cmake/FindTinyXML.cmake # - Find TinyXML # Find the native TinyXML includes and library # # TINYXML_FOUND - True if TinyXML found. # TINYXML_INCLUDE_DIR - where to find tinyxml.h, etc. # TINYXML_LIBRARIES - List of libraries when using TinyXML. # INCLUDE( "FindPackageHandleStandardArgs" ) FIND_PATH( TINYXML_INCLUDE_DIRS "tinyxml.h" PATH_SUFFIXES "tinyxml" ) FIND_LIBRARY( TINYXML_LIBRARY_DIRS NAMES "tinyxml" PATH_SUFFIXES "tinyxml" ) # handle the QUIETLY and REQUIRED arguments and set TINYXML_FOUND to TRUE if # all listed variables are TRUE FIND_PACKAGE_HANDLE_STANDARD_ARGS( "TinyXML" DEFAULT_MSG TINYXML_INCLUDE_DIRS TINYXML_LIBRARY_DIRS ) fcl-0.5.0/CMakeModules/cmake_uninstall.cmake.in000066400000000000000000000016371274356571000213720ustar00rootroot00000000000000if(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") message(FATAL_ERROR "Cannot find install manifest: @CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") endif(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") file(READ "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt" files) string(REGEX REPLACE "\n" ";" files "${files}") foreach(file ${files}) message(STATUS "Uninstalling $ENV{DESTDIR}${file}") if(EXISTS "$ENV{DESTDIR}${file}") execute_process( COMMAND @CMAKE_COMMAND@ -E remove "$ENV{DESTDIR}${file}" OUTPUT_VARIABLE rm_out RESULT_VARIABLE rm_retval ) if(NOT "${rm_retval}" STREQUAL 0) message(FATAL_ERROR "Problem when removing $ENV{DESTDIR}${file}") endif(NOT "${rm_retval}" STREQUAL 0) else(EXISTS "$ENV{DESTDIR}${file}") message(STATUS "File $ENV{DESTDIR}${file} does not exist.") endif(EXISTS "$ENV{DESTDIR}${file}") endforeach(file) fcl-0.5.0/INSTALL000066400000000000000000000017001274356571000133210ustar00rootroot00000000000000 Dependencies: ============ - Boost (thread, date_time, unit_test_framework, filesystem) - libccd (available at http://libccd.danfis.cz/) - octomap (optional dependency, available at http://octomap.github.com) Boost and libccd are mandatory dependencies. If octomap is not found, collision detection with octrees will not be possible. For installation, CMake will also be needed (http://cmake.org). Install: ======= * Linux / Mac OS: The CMakeLists.txt can be used to generate makefiles; For example, one may use operations such as: mkdir build cd build cmake .. make -jN # N is the maximum number of parallel compile jobs Once the compilation is finished, make install will install the project. To specify the installation prefix, pass the parameter -DCMAKE_INSTALL_PREFIX=/my/prefix/ to the "cmake .." command above. * Visual Studio: The CMakeLists.txt can be used to generate a Visual Studio project, using the cmake build tool. fcl-0.5.0/LICENSE000066400000000000000000000031571274356571000133050ustar00rootroot00000000000000Software License Agreement (BSD License) Copyright (c) 2008-2014, Willow Garage, Inc. Copyright (c) 2014-2016, Open Source Robotics Foundation All rights reserved. Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. * 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. * Neither the name of Open Source Robotics Foundation 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 OWNER 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. fcl-0.5.0/README.md000066400000000000000000000230641274356571000135560ustar00rootroot00000000000000## FCL -- The Flexible Collision Library Linux / OS X [![Build Status](https://travis-ci.org/flexible-collision-library/fcl.svg)](https://travis-ci.org/flexible-collision-library/fcl) Windows [![Build status](https://ci.appveyor.com/api/projects/status/rrmxadnj1empitqq?svg=true)](https://ci.appveyor.com/project/mamoll/fcl) Coverage [![Coverage Status](https://coveralls.io/repos/github/flexible-collision-library/fcl/badge.svg?branch=master)](https://coveralls.io/github/flexible-collision-library/fcl?branch=master) FCL is a library for performing three types of proximity queries on a pair of geometric models composed of triangles. - Collision detection: detecting whether the two models overlap, and optionally, all of the triangles that overlap. - Distance computation: computing the minimum distance between a pair of models, i.e., the distance between the closest pair of points. - Tolerance verification: determining whether two models are closer or farther than a tolerance distance. - Continuous collision detection: detecting whether the two moving models overlap during the movement, and optionally, the time of contact. - Contact information: for collision detection and continuous collision detection, the contact information (including contact normals and contact points) can be returned optionally. FCL has the following features - C++ interface - Compilable for either linux or win32 (both makefiles and Microsoft Visual projects can be generated using cmake) - No special topological constraints or adjacency information required for input models – all that is necessary is a list of the model's triangles - Supported different object shapes: + box + sphere + ellipsoid + capsule + cone + cylinder + convex + half-space + plane + mesh + octree (optional, octrees are represented using the octomap library http://octomap.github.com) ## Installation Before compiling FCL, please make libccd (for collision checking between convex objects and is available here https://github.com/danfis/libccd) is installed. For libccd, make sure to compile from github version instead of the zip file from the webpage, because one bug fixing is not included in the zipped version. To compile the unit tests, Boost also needs to be to installed. Some optional libraries need to be installed for some optional capability of FCL. For octree collision, please install the octomap library from http://octomap.github.com. CMakeLists.txt is used to generate makefiles in Linux or Visual studio projects in windows. In command line, run ``` cmake mkdir build cd build cmake .. ``` Next, in linux, use make to compile the code. In windows, there will generate a visual studio project and then you can compile the code. ## Interfaces Before starting the proximity computation, we need first to set the geometry and transform for the objects involving in computation. The geometry of an object is represented as a mesh soup, which can be set as follows: ```cpp // set mesh triangles and vertice indices std::vector vertices; std::vector triangles; // code to set the vertices and triangles ... // BVHModel is a template class for mesh geometry, for default OBBRSS template is used typedef BVHModel Model; Model* model = new Model(); // add the mesh data into the BVHModel structure model->beginModel(); model->addSubModel(vertices, triangles); model->endModel(); ``` The transform of an object includes the rotation and translation: ```cpp // R and T are the rotation matrix and translation vector Matrix3f R; Vec3f T; // code for setting R and T ... // transform is configured according to R and T Transform3f pose(R, T); ``` Given the geometry and the transform, we can also combine them together to obtain a collision object instance and here is an example: ```cpp //geom and tf are the geometry and the transform of the object BVHModel* geom = ... Transform3f tf = ... //Combine them together CollisionObject* obj = new CollisionObject(geom, tf); ``` Once the objects are set, we can perform the proximity computation between them. All the proximity queries in FCL follow a common pipeline: first, set the query request data structure and then run the query function by using request as the input. The result is returned in a query result data structure. For example, for collision checking, we first set the CollisionRequest data structure, and then run the collision function: ```cpp // Given two objects o1 and o2 CollisionObject* o1 = ... CollisionObject* o2 = ... // set the collision request structure, here we just use the default setting CollisionRequest request; // result will be returned via the collision result structure CollisionResult result; // perform collision test collide(o1, o2, request, result); ``` By setting the collision request, the user can easily choose whether to return contact information (which is slower) or just return binary collision results (which is faster). For distance computation, the pipeline is almost the same: ```cpp // Given two objects o1 and o2 CollisionObject* o1 = ... CollisionObject* o2 = ... // set the distance request structure, here we just use the default setting DistanceRequest request; // result will be returned via the collision result structure DistanceResult result; // perform distance test distance(o1, o2, request, result); ``` For continuous collision, FCL requires the goal transform to be provided (the initial transform is included in the collision object data structure). Beside that, the pipeline is almost the same as distance/collision: ```cpp // Given two objects o1 and o2 CollisionObject* o1 = ... CollisionObject* o2 = ... // The goal transforms for o1 and o2 Transform3f tf_goal_o1 = ... Transform3f tf_goal_o2 = ... // set the continuous collision request structure, here we just use the default setting ContinuousCollisionRequest request; // result will be returned via the continuous collision result structure ContinuousCollisionResult result; // perform continuous collision test continuousCollide(o1, tf_goal_o1, o2, tf_goal_o2, request, result); ``` FCL supports broadphase collision/distance between two groups of objects and can avoid the n square complexity. For collision, broadphase algorithm can return all the collision pairs. For distance, it can return the pair with the minimum distance. FCL uses a CollisionManager structure to manage all the objects involving the collision or distance operations. ```cpp // Initialize the collision manager for the first group of objects. // FCL provides various different implementations of CollisionManager. // Generally, the DynamicAABBTreeCollisionManager would provide the best performance. BroadPhaseCollisionManager* manager1 = new DynamicAABBTreeCollisionManager(); // Initialize the collision manager for the second group of objects. BroadPhaseCollisionManager* manager2 = new DynamicAABBTreeCollisionManager(); // To add objects into the collision manager, using BroadPhaseCollisionManager::registerObject() function to add one object std::vector objects1 = ... for(std::size_t i = 0; i < objects1.size(); ++i) manager1->registerObject(objects1[i]); // Another choose is to use BroadPhaseCollisionManager::registerObjects() function to add a set of objects std::vector objects2 = ... manager2->registerObjects(objects2); // In order to collect the information during broadphase, CollisionManager requires two settings: // a) a callback to collision or distance; // b) an intermediate data to store the information generated during the broadphase computation // For a), FCL provides the default callbacks for both collision and distance. // For b), FCL uses the CollisionData structure for collision and DistanceData structure for distance. CollisionData/DistanceData is just a container including both the CollisionRequest/DistanceRequest and CollisionResult/DistanceResult structures mentioned above. CollisionData collision_data; DistanceData distance_data; // Setup the managers, which is related with initializing the broadphase acceleration structure according to objects input manager1->setup(); manager2->setup(); // Examples for various queries // 1. Collision query between two object groups and get collision numbers manager2->collide(manager1, &collision_data, defaultCollisionFunction); int n_contact_num = collision_data.result.numContacts(); // 2. Distance query between two object groups and get the minimum distance manager2->distance(manager1, &distance_data, defaultDistanceFunction); double min_distance = distance_data.result.min_distance; // 3. Self collision query for group 1 manager1->collide(&collision_data, defaultCollisionFunction); // 4. Self distance query for group 1 manager1->distance(&distance_data, defaultDistanceFunction); // 5. Collision query between one object in group 1 and the entire group 2 manager2->collide(objects1[0], &collision_data, defaultCollisionFunction); // 6. Distance query between one object in group 1 and the entire group 2 manager2->distance(objects1[0], &distance_data, defaultDistanceFunction); ``` For more examples, please refer to the test folder: - test_fcl_collision.cpp: provide examples for collision test - test_fcl_distance.cpp: provide examples for distance test - test_fcl_broadphase.cpp: provide examples for broadphase collision/distance test - test_fcl_frontlist.cpp: provide examples for frontlist collision acceleration #- test_fcl_global_penetration.cpp: provide examples for global penetration depth test #- test_fcl_xmldata.cpp: provide examples for more global penetration depth test based on xml data - test_fcl_octomap.cpp: provide examples for collision/distance computation between octomap data and other data types. fcl-0.5.0/ci/000077500000000000000000000000001274356571000126655ustar00rootroot00000000000000fcl-0.5.0/ci/install_linux.sh000077500000000000000000000004221274356571000161070ustar00rootroot00000000000000sudo add-apt-repository --yes ppa:libccd-debs/ppa sudo apt-get -qq update sudo apt-get -qq --yes --force-yes install libccd-dev # Octomap git clone https://github.com/OctoMap/octomap cd octomap git checkout tags/v1.8.0 mkdir build cd build cmake .. make sudo make install fcl-0.5.0/ci/install_osx.sh000077500000000000000000000001461274356571000155640ustar00rootroot00000000000000brew tap homebrew/science brew install git brew install cmake brew install boost brew install libccd fcl-0.5.0/fcl.pc.in000066400000000000000000000005301274356571000137650ustar00rootroot00000000000000# This file was generated by CMake for @PROJECT_NAME@ prefix=@CMAKE_INSTALL_PREFIX@ exec_prefix=${prefix} libdir=@CMAKE_INSTALL_FULL_LIBDIR@ includedir=@CMAKE_INSTALL_FULL_INCLUDEDIR@ Name: @PROJECT_NAME@ Description: @PKG_DESC@ Version: @FCL_VERSION@ Requires: @PKG_EXTERNAL_DEPS@ Libs: -L${libdir} -lfcl Cflags: @PKG_CFLAGS@ -I${includedir} fcl-0.5.0/include/000077500000000000000000000000001274356571000137155ustar00rootroot00000000000000fcl-0.5.0/include/fcl/000077500000000000000000000000001274356571000144615ustar00rootroot00000000000000fcl-0.5.0/include/fcl/BV/000077500000000000000000000000001274356571000147705ustar00rootroot00000000000000fcl-0.5.0/include/fcl/BV/AABB.h000066400000000000000000000153221274356571000156310ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_AABB_H #define FCL_AABB_H #include "fcl/math/vec_3f.h" namespace fcl { /// @brief A class describing the AABB collision structure, which is a box in 3D space determined by two diagonal points class AABB { public: /// @brief The min point in the AABB Vec3f min_; /// @brief The max point in the AABB Vec3f max_; /// @brief Creating an AABB with zero size (low bound +inf, upper bound -inf) AABB(); /// @brief Creating an AABB at position v with zero size AABB(const Vec3f& v) : min_(v), max_(v) { } /// @brief Creating an AABB with two endpoints a and b AABB(const Vec3f& a, const Vec3f&b) : min_(min(a, b)), max_(max(a, b)) { } /// @brief Creating an AABB centered as core and is of half-dimension delta AABB(const AABB& core, const Vec3f& delta) : min_(core.min_ - delta), max_(core.max_ + delta) { } /// @brief Creating an AABB contains three points AABB(const Vec3f& a, const Vec3f& b, const Vec3f& c) : min_(min(min(a, b), c)), max_(max(max(a, b), c)) { } /// @brief Check whether two AABB are overlap inline bool overlap(const AABB& other) const { if(min_[0] > other.max_[0]) return false; if(min_[1] > other.max_[1]) return false; if(min_[2] > other.max_[2]) return false; if(max_[0] < other.min_[0]) return false; if(max_[1] < other.min_[1]) return false; if(max_[2] < other.min_[2]) return false; return true; } /// @brief Check whether the AABB contains another AABB inline bool contain(const AABB& other) const { return (other.min_[0] >= min_[0]) && (other.max_[0] <= max_[0]) && (other.min_[1] >= min_[1]) && (other.max_[1] <= max_[1]) && (other.min_[2] >= min_[2]) && (other.max_[2] <= max_[2]); } /// @brief Check whether two AABB are overlapped along specific axis inline bool axisOverlap(const AABB& other, int axis_id) const { if(min_[axis_id] > other.max_[axis_id]) return false; if(max_[axis_id] < other.min_[axis_id]) return false; return true; } /// @brief Check whether two AABB are overlap and return the overlap part inline bool overlap(const AABB& other, AABB& overlap_part) const { if(!overlap(other)) { return false; } overlap_part.min_ = max(min_, other.min_); overlap_part.max_ = min(max_, other.max_); return true; } /// @brief Check whether the AABB contains a point inline bool contain(const Vec3f& p) const { if(p[0] < min_[0] || p[0] > max_[0]) return false; if(p[1] < min_[1] || p[1] > max_[1]) return false; if(p[2] < min_[2] || p[2] > max_[2]) return false; return true; } /// @brief Merge the AABB and a point inline AABB& operator += (const Vec3f& p) { min_.ubound(p); max_.lbound(p); return *this; } /// @brief Merge the AABB and another AABB inline AABB& operator += (const AABB& other) { min_.ubound(other.min_); max_.lbound(other.max_); return *this; } /// @brief Return the merged AABB of current AABB and the other one inline AABB operator + (const AABB& other) const { AABB res(*this); return res += other; } /// @brief Width of the AABB inline FCL_REAL width() const { return max_[0] - min_[0]; } /// @brief Height of the AABB inline FCL_REAL height() const { return max_[1] - min_[1]; } /// @brief Depth of the AABB inline FCL_REAL depth() const { return max_[2] - min_[2]; } /// @brief Volume of the AABB inline FCL_REAL volume() const { return width() * height() * depth(); } /// @brief Size of the AABB (used in BV_Splitter to order two AABBs) inline FCL_REAL size() const { return (max_ - min_).sqrLength(); } /// @brief Radius of the AABB inline FCL_REAL radius() const { return (max_ - min_).length() / 2; } /// @brief Center of the AABB inline Vec3f center() const { return (min_ + max_) * 0.5; } /// @brief Distance between two AABBs; P and Q, should not be NULL, return the nearest points FCL_REAL distance(const AABB& other, Vec3f* P, Vec3f* Q) const; /// @brief Distance between two AABBs FCL_REAL distance(const AABB& other) const; /// @brief whether two AABB are equal inline bool equal(const AABB& other) const { return min_.equal(other.min_) && max_.equal(other.max_); } /// @brief expand the half size of the AABB by delta, and keep the center unchanged. inline AABB& expand(const Vec3f& delta) { min_ -= delta; max_ += delta; return *this; } /// @brief expand the aabb by increase the thickness of the plate by a ratio inline AABB& expand(const AABB& core, FCL_REAL ratio) { min_ = min_ * ratio - core.min_; max_ = max_ * ratio - core.max_; return *this; } }; /// @brief translate the center of AABB by t static inline AABB translate(const AABB& aabb, const Vec3f& t) { AABB res(aabb); res.min_ += t; res.max_ += t; return res; } } #endif fcl-0.5.0/include/fcl/BV/BV.h000066400000000000000000000203341274356571000154520ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_BV_H #define FCL_BV_H #include "fcl/BV/kDOP.h" #include "fcl/BV/AABB.h" #include "fcl/BV/OBB.h" #include "fcl/BV/RSS.h" #include "fcl/BV/OBBRSS.h" #include "fcl/BV/kIOS.h" #include "fcl/math/transform.h" /** \brief Main namespace */ namespace fcl { /// @cond IGNORE namespace details { /// @brief Convert a bounding volume of type BV1 in configuration tf1 to a bounding volume of type BV2 in I configuration. template class Converter { private: static void convert(const BV1& bv1, const Transform3f& tf1, BV2& bv2) { // should only use the specialized version, so it is private. } }; /// @brief Convert from AABB to AABB, not very tight but is fast. template<> class Converter { public: static void convert(const AABB& bv1, const Transform3f& tf1, AABB& bv2) { const Vec3f& center = bv1.center(); FCL_REAL r = (bv1.max_ - bv1.min_).length() * 0.5; Vec3f center2 = tf1.transform(center); Vec3f delta(r, r, r); bv2.min_ = center2 - delta; bv2.max_ = center2 + delta; } }; template<> class Converter { public: static void convert(const AABB& bv1, const Transform3f& tf1, OBB& bv2) { /* bv2.To = tf1.transform(bv1.center()); /// Sort the AABB edges so that AABB extents are ordered. FCL_REAL d[3] = {bv1.width(), bv1.height(), bv1.depth() }; std::size_t id[3] = {0, 1, 2}; for(std::size_t i = 1; i < 3; ++i) { for(std::size_t j = i; j > 0; --j) { if(d[j] > d[j-1]) { { FCL_REAL tmp = d[j]; d[j] = d[j-1]; d[j-1] = tmp; } { std::size_t tmp = id[j]; id[j] = id[j-1]; id[j-1] = tmp; } } } } Vec3f extent = (bv1.max_ - bv1.min_) * 0.5; bv2.extent = Vec3f(extent[id[0]], extent[id[1]], extent[id[2]]); const Matrix3f& R = tf1.getRotation(); bool left_hand = (id[0] == (id[1] + 1) % 3); bv2.axis[0] = left_hand ? -R.getColumn(id[0]) : R.getColumn(id[0]); bv2.axis[1] = R.getColumn(id[1]); bv2.axis[2] = R.getColumn(id[2]); */ bv2.To = tf1.transform(bv1.center()); bv2.extent = (bv1.max_ - bv1.min_) * 0.5; const Matrix3f& R = tf1.getRotation(); bv2.axis[0] = R.getColumn(0); bv2.axis[1] = R.getColumn(1); bv2.axis[2] = R.getColumn(2); } }; template<> class Converter { public: static void convert(const OBB& bv1, const Transform3f& tf1, OBB& bv2) { bv2.extent = bv1.extent; bv2.To = tf1.transform(bv1.To); bv2.axis[0] = tf1.getQuatRotation().transform(bv1.axis[0]); bv2.axis[1] = tf1.getQuatRotation().transform(bv1.axis[1]); bv2.axis[2] = tf1.getQuatRotation().transform(bv1.axis[2]); } }; template<> class Converter { public: static void convert(const OBBRSS& bv1, const Transform3f& tf1, OBB& bv2) { Converter::convert(bv1.obb, tf1, bv2); } }; template<> class Converter { public: static void convert(const RSS& bv1, const Transform3f& tf1, OBB& bv2) { bv2.extent = Vec3f(bv1.l[0] * 0.5 + bv1.r, bv1.l[1] * 0.5 + bv1.r, bv1.r); bv2.To = tf1.transform(bv1.Tr); bv2.axis[0] = tf1.getQuatRotation().transform(bv1.axis[0]); bv2.axis[1] = tf1.getQuatRotation().transform(bv1.axis[1]); bv2.axis[2] = tf1.getQuatRotation().transform(bv1.axis[2]); } }; template class Converter { public: static void convert(const BV1& bv1, const Transform3f& tf1, AABB& bv2) { const Vec3f& center = bv1.center(); FCL_REAL r = Vec3f(bv1.width(), bv1.height(), bv1.depth()).length() * 0.5; Vec3f delta(r, r, r); Vec3f center2 = tf1.transform(center); bv2.min_ = center2 - delta; bv2.max_ = center2 + delta; } }; template class Converter { public: static void convert(const BV1& bv1, const Transform3f& tf1, OBB& bv2) { AABB bv; Converter::convert(bv1, Transform3f(), bv); Converter::convert(bv, tf1, bv2); } }; template<> class Converter { public: static void convert(const OBB& bv1, const Transform3f& tf1, RSS& bv2) { bv2.Tr = tf1.transform(bv1.To); bv2.axis[0] = tf1.getQuatRotation().transform(bv1.axis[0]); bv2.axis[1] = tf1.getQuatRotation().transform(bv1.axis[1]); bv2.axis[2] = tf1.getQuatRotation().transform(bv1.axis[2]); bv2.r = bv1.extent[2]; bv2.l[0] = 2 * (bv1.extent[0] - bv2.r); bv2.l[1] = 2 * (bv1.extent[1] - bv2.r); } }; template<> class Converter { public: static void convert(const RSS& bv1, const Transform3f& tf1, RSS& bv2) { bv2.Tr = tf1.transform(bv1.Tr); bv2.axis[0] = tf1.getQuatRotation().transform(bv1.axis[0]); bv2.axis[1] = tf1.getQuatRotation().transform(bv1.axis[1]); bv2.axis[2] = tf1.getQuatRotation().transform(bv1.axis[2]); bv2.r = bv1.r; bv2.l[0] = bv1.l[0]; bv2.l[1] = bv1.l[1]; } }; template<> class Converter { public: static void convert(const OBBRSS& bv1, const Transform3f& tf1, RSS& bv2) { Converter::convert(bv1.rss, tf1, bv2); } }; template<> class Converter { public: static void convert(const AABB& bv1, const Transform3f& tf1, RSS& bv2) { bv2.Tr = tf1.transform(bv1.center()); /// Sort the AABB edges so that AABB extents are ordered. FCL_REAL d[3] = {bv1.width(), bv1.height(), bv1.depth() }; std::size_t id[3] = {0, 1, 2}; for(std::size_t i = 1; i < 3; ++i) { for(std::size_t j = i; j > 0; --j) { if(d[j] > d[j-1]) { { FCL_REAL tmp = d[j]; d[j] = d[j-1]; d[j-1] = tmp; } { std::size_t tmp = id[j]; id[j] = id[j-1]; id[j-1] = tmp; } } } } Vec3f extent = (bv1.max_ - bv1.min_) * 0.5; bv2.r = extent[id[2]]; bv2.l[0] = (extent[id[0]] - bv2.r) * 2; bv2.l[1] = (extent[id[1]] - bv2.r) * 2; const Matrix3f& R = tf1.getRotation(); bool left_hand = (id[0] == (id[1] + 1) % 3); bv2.axis[0] = left_hand ? -R.getColumn(id[0]) : R.getColumn(id[0]); bv2.axis[1] = R.getColumn(id[1]); bv2.axis[2] = R.getColumn(id[2]); } }; } /// @endcond /// @brief Convert a bounding volume of type BV1 in configuration tf1 to bounding volume of type BV2 in identity configuration. template static inline void convertBV(const BV1& bv1, const Transform3f& tf1, BV2& bv2) { details::Converter::convert(bv1, tf1, bv2); } } #endif fcl-0.5.0/include/fcl/BV/BV_node.h000066400000000000000000000116361274356571000164640ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_BV_NODE_H #define FCL_BV_NODE_H #include "fcl/math/vec_3f.h" #include "fcl/math/matrix_3f.h" #include "fcl/BV/BV.h" #include namespace fcl { /// @brief BVNodeBase encodes the tree structure for BVH struct BVNodeBase { /// @brief An index for first child node or primitive /// If the value is positive, it is the index of the first child bv node /// If the value is negative, it is -(primitive index + 1) /// Zero is not used. int first_child; /// @brief The start id the primitive belonging to the current node. The index is referred to the primitive_indices in BVHModel and from that /// we can obtain the primitive's index in original data indirectly. int first_primitive; /// @brief The number of primitives belonging to the current node int num_primitives; /// @brief Whether current node is a leaf node (i.e. contains a primitive index inline bool isLeaf() const { return first_child < 0; } /// @brief Return the primitive index. The index is referred to the original data (i.e. vertices or tri_indices) in BVHModel inline int primitiveId() const { return -(first_child + 1); } /// @brief Return the index of the first child. The index is referred to the bounding volume array (i.e. bvs) in BVHModel inline int leftChild() const { return first_child; } /// @brief Return the index of the second child. The index is referred to the bounding volume array (i.e. bvs) in BVHModel inline int rightChild() const { return first_child + 1; } }; /// @brief A class describing a bounding volume node. It includes the tree structure providing in BVNodeBase and also the geometry data provided in BV template parameter. template struct BVNode : public BVNodeBase { /// @brief bounding volume storing the geometry BV bv; /// @brief Check whether two BVNode collide bool overlap(const BVNode& other) const { return bv.overlap(other.bv); } /// @brief Compute the distance between two BVNode. P1 and P2, if not NULL and the underlying BV supports distance, return the nearest points. FCL_REAL distance(const BVNode& other, Vec3f* P1 = NULL, Vec3f* P2 = NULL) const { return bv.distance(other.bv, P1, P2); } /// @brief Access the center of the BV Vec3f getCenter() const { return bv.center(); } /// @brief Access the orientation of the BV Matrix3f getOrientation() const { return Matrix3f::getIdentity(); } }; template<> inline Matrix3f BVNode::getOrientation() const { return Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0], bv.axis[0][1], bv.axis[1][1], bv.axis[2][1], bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]); } template<> inline Matrix3f BVNode::getOrientation() const { return Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0], bv.axis[0][1], bv.axis[1][1], bv.axis[2][1], bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]); } template<> inline Matrix3f BVNode::getOrientation() const { return Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0], bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1], bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]); } } #endif fcl-0.5.0/include/fcl/BV/OBB.h000066400000000000000000000110431274356571000155420ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_OBB_H #define FCL_OBB_H #include "fcl/math/vec_3f.h" #include "fcl/math/matrix_3f.h" namespace fcl { /// @brief Oriented bounding box class class OBB { public: /// @brief Orientation of OBB. axis[i] is the ith column of the orientation matrix for the box; it is also the i-th principle direction of the box. /// We assume that axis[0] corresponds to the axis with the longest box edge, axis[1] corresponds to the shorter one and axis[2] corresponds to the shortest one. Vec3f axis[3]; /// @brief Center of OBB Vec3f To; /// @brief Half dimensions of OBB Vec3f extent; /// @brief Check collision between two OBB, return true if collision happens. bool overlap(const OBB& other) const; /// @brief Check collision between two OBB and return the overlap part. For OBB, the overlap_part return value is NOT used as the overlap part of two obbs usually is not an obb. bool overlap(const OBB& other, OBB& overlap_part) const { return overlap(other); } /// @brief Check whether the OBB contains a point. bool contain(const Vec3f& p) const; /// @brief A simple way to merge the OBB and a point (the result is not compact). OBB& operator += (const Vec3f& p); /// @brief Merge the OBB and another OBB (the result is not compact). OBB& operator += (const OBB& other) { *this = *this + other; return *this; } /// @brief Return the merged OBB of current OBB and the other one (the result is not compact). OBB operator + (const OBB& other) const; /// @brief Width of the OBB. inline FCL_REAL width() const { return 2 * extent[0]; } /// @brief Height of the OBB. inline FCL_REAL height() const { return 2 * extent[1]; } /// @brief Depth of the OBB inline FCL_REAL depth() const { return 2 * extent[2]; } /// @brief Volume of the OBB inline FCL_REAL volume() const { return width() * height() * depth(); } /// @brief Size of the OBB (used in BV_Splitter to order two OBBs) inline FCL_REAL size() const { return extent.sqrLength(); } /// @brief Center of the OBB inline const Vec3f& center() const { return To; } /// @brief Distance between two OBBs, not implemented. FCL_REAL distance(const OBB& other, Vec3f* P = NULL, Vec3f* Q = NULL) const; }; /// @brief Translate the OBB bv OBB translate(const OBB& bv, const Vec3f& t); /// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and b2 is in identity. bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2); /// @brief Check collision between two boxes: the first box is in configuration (R, T) and its half dimension is set by a; /// the second box is in identity configuration and its half dimension is set by b. bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b); } #endif fcl-0.5.0/include/fcl/BV/OBBRSS.h000066400000000000000000000105251274356571000161360ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_OBBRSS_H #define FCL_OBBRSS_H #include "fcl/BV/OBB.h" #include "fcl/BV/RSS.h" namespace fcl { /// @brief Class merging the OBB and RSS, can handle collision and distance simultaneously class OBBRSS { public: /// @brief OBB member, for rotation OBB obb; /// @brief RSS member, for distance RSS rss; /// @brief Check collision between two OBBRSS bool overlap(const OBBRSS& other) const { return obb.overlap(other.obb); } /// @brief Check collision between two OBBRSS and return the overlap part. bool overlap(const OBBRSS& other, OBBRSS& overlap_part) const { return overlap(other); } /// @brief Check whether the OBBRSS contains a point inline bool contain(const Vec3f& p) const { return obb.contain(p); } /// @brief Merge the OBBRSS and a point OBBRSS& operator += (const Vec3f& p) { obb += p; rss += p; return *this; } /// @brief Merge two OBBRSS OBBRSS& operator += (const OBBRSS& other) { *this = *this + other; return *this; } /// @brief Merge two OBBRSS OBBRSS operator + (const OBBRSS& other) const { OBBRSS result; result.obb = obb + other.obb; result.rss = rss + other.rss; return result; } /// @brief Width of the OBRSS inline FCL_REAL width() const { return obb.width(); } /// @brief Height of the OBBRSS inline FCL_REAL height() const { return obb.height(); } /// @brief Depth of the OBBRSS inline FCL_REAL depth() const { return obb.depth(); } /// @brief Volume of the OBBRSS inline FCL_REAL volume() const { return obb.volume(); } /// @brief Size of the OBBRSS (used in BV_Splitter to order two OBBRSS) inline FCL_REAL size() const { return obb.size(); } /// @brief Center of the OBBRSS inline const Vec3f& center() const { return obb.center(); } /// @brief Distance between two OBBRSS; P and Q , is not NULL, returns the nearest points FCL_REAL distance(const OBBRSS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const { return rss.distance(other.rss, P, Q); } }; /// @brief Translate the OBBRSS bv OBBRSS translate(const OBBRSS& bv, const Vec3f& t); /// @brief Check collision between two OBBRSS, b1 is in configuration (R0, T0) and b2 is in indentity bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2); /// @brief Computate distance between two OBBRSS, b1 is in configuation (R0, T0) and b2 is in indentity; P and Q, is not NULL, returns the nearest points FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2, Vec3f* P = NULL, Vec3f* Q = NULL); } #endif fcl-0.5.0/include/fcl/BV/RSS.h000066400000000000000000000115421274356571000156130ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_RSS_H #define FCL_RSS_H #include "fcl/math/constants.h" #include "fcl/math/vec_3f.h" #include "fcl/math/matrix_3f.h" namespace fcl { /// @brief A class for rectangle sphere-swept bounding volume class RSS { public: /// @brief Orientation of RSS. axis[i] is the ith column of the orientation matrix for the RSS; it is also the i-th principle direction of the RSS. /// We assume that axis[0] corresponds to the axis with the longest length, axis[1] corresponds to the shorter one and axis[2] corresponds to the shortest one. Vec3f axis[3]; /// @brief Origin of the rectangle in RSS Vec3f Tr; /// @brief Side lengths of rectangle FCL_REAL l[2]; /// @brief Radius of sphere summed with rectangle to form RSS FCL_REAL r; /// @brief Check collision between two RSS bool overlap(const RSS& other) const; /// @brief Check collision between two RSS and return the overlap part. /// For RSS, we return nothing, as the overlap part of two RSSs usually is not a RSS. bool overlap(const RSS& other, RSS& overlap_part) const { return overlap(other); } /// @brief Check whether the RSS contains a point inline bool contain(const Vec3f& p) const; /// @brief A simple way to merge the RSS and a point, not compact. /// @todo This function may have some bug. RSS& operator += (const Vec3f& p); /// @brief Merge the RSS and another RSS inline RSS& operator += (const RSS& other) { *this = *this + other; return *this; } /// @brief Return the merged RSS of current RSS and the other one RSS operator + (const RSS& other) const; /// @brief Width of the RSS inline FCL_REAL width() const { return l[0] + 2 * r; } /// @brief Height of the RSS inline FCL_REAL height() const { return l[1] + 2 * r; } /// @brief Depth of the RSS inline FCL_REAL depth() const { return 2 * r; } /// @brief Volume of the RSS inline FCL_REAL volume() const { return (l[0] * l[1] * 2 * r + 4 * constants::pi * r * r * r); } /// @brief Size of the RSS (used in BV_Splitter to order two RSSs) inline FCL_REAL size() const { return (std::sqrt(l[0] * l[0] + l[1] * l[1]) + 2 * r); } /// @brief The RSS center inline const Vec3f& center() const { return Tr; } /// @brief the distance between two RSS; P and Q, if not NULL, return the nearest points FCL_REAL distance(const RSS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const; }; /// @brief Translate the RSS bv RSS translate(const RSS& bv, const Vec3f& t); /// @brief distance between two RSS bounding volumes /// P and Q (optional return values) are the closest points in the rectangles, not the RSS. But the direction P - Q is the correct direction for cloest points /// Notice that P and Q are both in the local frame of the first RSS (not global frame and not even the local frame of object 1) FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2, Vec3f* P = NULL, Vec3f* Q = NULL); /// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and b2 is in identity. bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2); } #endif fcl-0.5.0/include/fcl/BV/kDOP.h000066400000000000000000000132171274356571000157420ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_KDOP_H #define FCL_KDOP_H #include "fcl/math/vec_3f.h" namespace fcl { /// @brief KDOP class describes the KDOP collision structures. K is set as the template parameter, which should be 16, 18, or 24 /// The KDOP structure is defined by some pairs of parallel planes defined by some axes. /// For K = 16, the planes are 6 AABB planes and 10 diagonal planes that cut off some space of the edges: /// (-1,0,0) and (1,0,0) -> indices 0 and 8 /// (0,-1,0) and (0,1,0) -> indices 1 and 9 /// (0,0,-1) and (0,0,1) -> indices 2 and 10 /// (-1,-1,0) and (1,1,0) -> indices 3 and 11 /// (-1,0,-1) and (1,0,1) -> indices 4 and 12 /// (0,-1,-1) and (0,1,1) -> indices 5 and 13 /// (-1,1,0) and (1,-1,0) -> indices 6 and 14 /// (-1,0,1) and (1,0,-1) -> indices 7 and 15 /// For K = 18, the planes are 6 AABB planes and 12 diagonal planes that cut off some space of the edges: /// (-1,0,0) and (1,0,0) -> indices 0 and 9 /// (0,-1,0) and (0,1,0) -> indices 1 and 10 /// (0,0,-1) and (0,0,1) -> indices 2 and 11 /// (-1,-1,0) and (1,1,0) -> indices 3 and 12 /// (-1,0,-1) and (1,0,1) -> indices 4 and 13 /// (0,-1,-1) and (0,1,1) -> indices 5 and 14 /// (-1,1,0) and (1,-1,0) -> indices 6 and 15 /// (-1,0,1) and (1,0,-1) -> indices 7 and 16 /// (0,-1,1) and (0,1,-1) -> indices 8 and 17 /// For K = 18, the planes are 6 AABB planes and 18 diagonal planes that cut off some space of the edges: /// (-1,0,0) and (1,0,0) -> indices 0 and 12 /// (0,-1,0) and (0,1,0) -> indices 1 and 13 /// (0,0,-1) and (0,0,1) -> indices 2 and 14 /// (-1,-1,0) and (1,1,0) -> indices 3 and 15 /// (-1,0,-1) and (1,0,1) -> indices 4 and 16 /// (0,-1,-1) and (0,1,1) -> indices 5 and 17 /// (-1,1,0) and (1,-1,0) -> indices 6 and 18 /// (-1,0,1) and (1,0,-1) -> indices 7 and 19 /// (0,-1,1) and (0,1,-1) -> indices 8 and 20 /// (-1, -1, 1) and (1, 1, -1) --> indices 9 and 21 /// (-1, 1, -1) and (1, -1, 1) --> indices 10 and 22 /// (1, -1, -1) and (-1, 1, 1) --> indices 11 and 23 template class KDOP { public: /// @brief Creating kDOP containing nothing KDOP(); /// @brief Creating kDOP containing only one point KDOP(const Vec3f& v); /// @brief Creating kDOP containing two points KDOP(const Vec3f& a, const Vec3f& b); /// @brief Check whether two KDOPs are overlapped bool overlap(const KDOP& other) const; //// @brief Check whether one point is inside the KDOP bool inside(const Vec3f& p) const; /// @brief Merge the point and the KDOP KDOP& operator += (const Vec3f& p); /// @brief Merge two KDOPs KDOP& operator += (const KDOP& other); /// @brief Create a KDOP by mergin two KDOPs KDOP operator + (const KDOP& other) const; /// @brief The (AABB) width inline FCL_REAL width() const { return dist_[N / 2] - dist_[0]; } /// @brief The (AABB) height inline FCL_REAL height() const { return dist_[N / 2 + 1] - dist_[1]; } /// @brief The (AABB) depth inline FCL_REAL depth() const { return dist_[N / 2 + 2] - dist_[2]; } /// @brief The (AABB) volume inline FCL_REAL volume() const { return width() * height() * depth(); } /// @brief Size of the kDOP (used in BV_Splitter to order two kDOPs) inline FCL_REAL size() const { return width() * width() + height() * height() + depth() * depth(); } /// @brief The (AABB) center inline Vec3f center() const { return Vec3f(dist_[0] + dist_[N / 2], dist_[1] + dist_[N / 2 + 1], dist_[2] + dist_[N / 2 + 2]) * 0.5; } /// @brief The distance between two KDOP. Not implemented. FCL_REAL distance(const KDOP& other, Vec3f* P = NULL, Vec3f* Q = NULL) const; private: /// @brief Origin's distances to N KDOP planes FCL_REAL dist_[N]; public: inline FCL_REAL dist(std::size_t i) const { return dist_[i]; } inline FCL_REAL& dist(std::size_t i) { return dist_[i]; } }; /// @brief translate the KDOP BV template KDOP translate(const KDOP& bv, const Vec3f& t); } #endif fcl-0.5.0/include/fcl/BV/kIOS.h000066400000000000000000000115341274356571000157520ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_KIOS_H #define FCL_KIOS_H #include "fcl/BV/OBB.h" namespace fcl { /// @brief A class describing the kIOS collision structure, which is a set of spheres. class kIOS { /// @brief One sphere in kIOS struct kIOS_Sphere { Vec3f o; FCL_REAL r; }; /// @brief generate one sphere enclosing two spheres static kIOS_Sphere encloseSphere(const kIOS_Sphere& s0, const kIOS_Sphere& s1) { Vec3f d = s1.o - s0.o; FCL_REAL dist2 = d.sqrLength(); FCL_REAL diff_r = s1.r - s0.r; /** The sphere with the larger radius encloses the other */ if(diff_r * diff_r >= dist2) { if(s1.r > s0.r) return s1; else return s0; } else /** spheres partially overlapping or disjoint */ { float dist = std::sqrt(dist2); kIOS_Sphere s; s.r = dist + s0.r + s1.r; if(dist > 0) s.o = s0.o + d * ((s.r - s0.r) / dist); else s.o = s0.o; return s; } } public: /// @brief The (at most) five spheres for intersection kIOS_Sphere spheres[5]; /// @brief The number of spheres, no larger than 5 unsigned int num_spheres; /// @ OBB related with kIOS OBB obb; /// @brief Check collision between two kIOS bool overlap(const kIOS& other) const; /// @brief Check collision between two kIOS and return the overlap part. /// For kIOS, we return nothing, as the overlappart of two kIOS usually is not an kIOS /// @todo Not efficient. It first checks the sphere collisions and then use OBB for further culling. bool overlap(const kIOS& other, kIOS& overlap_part) const { return overlap(other); } /// @brief Check whether the kIOS contains a point inline bool contain(const Vec3f& p) const; /// @brief A simple way to merge the kIOS and a point kIOS& operator += (const Vec3f& p); /// @brief Merge the kIOS and another kIOS kIOS& operator += (const kIOS& other) { *this = *this + other; return *this; } /// @brief Return the merged kIOS of current kIOS and the other one kIOS operator + (const kIOS& other) const; /// @brief Center of the kIOS const Vec3f& center() const { return spheres[0].o; } /// @brief Width of the kIOS FCL_REAL width() const; /// @brief Height of the kIOS FCL_REAL height() const; /// @brief Depth of the kIOS FCL_REAL depth() const; /// @brief Volume of the kIOS FCL_REAL volume() const; /// @brief size of the kIOS (used in BV_Splitter to order two kIOSs) FCL_REAL size() const; /// @brief The distance between two kIOS FCL_REAL distance(const kIOS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const; }; /// @brief Translate the kIOS BV kIOS translate(const kIOS& bv, const Vec3f& t); /// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0) and b2 is in identity. /// @todo Not efficient bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2); /// @brief Approximate distance between two kIOS bounding volumes /// @todo P and Q is not returned, need implementation FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2, Vec3f* P = NULL, Vec3f* Q = NULL); } #endif fcl-0.5.0/include/fcl/BVH/000077500000000000000000000000001274356571000151005ustar00rootroot00000000000000fcl-0.5.0/include/fcl/BVH/BVH_front.h000066400000000000000000000054741274356571000171120ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_BVH_FRONT_H #define FCL_BVH_FRONT_H #include namespace fcl { /// @brief Front list acceleration for collision /// Front list is a set of internal and leaf nodes in the BVTT hierarchy, where /// the traversal terminates while performing a query during a given time instance. The front list reflects the subset of a /// BVTT that is traversed for that particular proximity query. struct BVHFrontNode { /// @brief The nodes to start in the future, i.e. the wave front of the traversal tree. int left, right; /// @brief The front node is not valid when collision is detected on the front node. bool valid; BVHFrontNode(int left_, int right_) : left(left_), right(right_), valid(true) { } }; /// @brief BVH front list is a list of front nodes. typedef std::list BVHFrontList; /// @brief Add new front node into the front list inline void updateFrontList(BVHFrontList* front_list, int b1, int b2) { if(front_list) front_list->push_back(BVHFrontNode(b1, b2)); } } #endif fcl-0.5.0/include/fcl/BVH/BVH_internal.h000066400000000000000000000072561274356571000175760ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_BVH_INTERNAL_H #define FCL_BVH_INTERNAL_H #include "fcl/data_types.h" namespace fcl { /// @brief States for BVH construction /// empty->begun->processed ->replace_begun->processed -> ...... /// | /// |-> update_begun -> updated -> ..... enum BVHBuildState { BVH_BUILD_STATE_EMPTY, /// empty state, immediately after constructor BVH_BUILD_STATE_BEGUN, /// after beginModel(), state for adding geometry primitives BVH_BUILD_STATE_PROCESSED, /// after tree has been build, ready for cd use BVH_BUILD_STATE_UPDATE_BEGUN, /// after beginUpdateModel(), state for updating geometry primitives BVH_BUILD_STATE_UPDATED, /// after tree has been build for updated geometry, ready for ccd use BVH_BUILD_STATE_REPLACE_BEGUN, /// after beginReplaceModel(), state for replacing geometry primitives }; /// @brief Error code for BVH enum BVHReturnCode { BVH_OK = 0, /// BVH is valid BVH_ERR_MODEL_OUT_OF_MEMORY = -1, /// Cannot allocate memory for vertices and triangles BVH_ERR_BUILD_OUT_OF_SEQUENCE = -2, /// BVH construction does not follow correct sequence BVH_ERR_BUILD_EMPTY_MODEL = -3, /// BVH geometry is not prepared BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME = -4, /// BVH geometry in previous frame is not prepared BVH_ERR_UNSUPPORTED_FUNCTION = -5, /// BVH funtion is not supported BVH_ERR_UNUPDATED_MODEL = -6, /// BVH model update failed BVH_ERR_INCORRECT_DATA = -7, /// BVH data is not valid BVH_ERR_UNKNOWN = -8 /// Unknown failure }; /// @brief BVH model type enum BVHModelType { BVH_MODEL_UNKNOWN, /// @brief unknown model type BVH_MODEL_TRIANGLES, /// @brief triangle model BVH_MODEL_POINTCLOUD /// @brief point cloud model }; } #endif fcl-0.5.0/include/fcl/BVH/BVH_model.h000066400000000000000000000261241274356571000170550ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_BVH_MODEL_H #define FCL_BVH_MODEL_H #include "fcl/collision_object.h" #include "fcl/BVH/BVH_internal.h" #include "fcl/BV/BV_node.h" #include "fcl/BVH/BV_splitter.h" #include "fcl/BVH/BV_fitter.h" #include #include namespace fcl { /// @brief A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as a degraded version of mesh) template class BVHModel : public CollisionGeometry { public: /// @brief Model type described by the instance BVHModelType getModelType() const { if(num_tris && num_vertices) return BVH_MODEL_TRIANGLES; else if(num_vertices) return BVH_MODEL_POINTCLOUD; else return BVH_MODEL_UNKNOWN; } /// @brief Constructing an empty BVH BVHModel() : vertices(NULL), tri_indices(NULL), prev_vertices(NULL), num_tris(0), num_vertices(0), build_state(BVH_BUILD_STATE_EMPTY), bv_splitter(new BVSplitter(SPLIT_METHOD_MEAN)), bv_fitter(new BVFitter()), num_tris_allocated(0), num_vertices_allocated(0), num_bvs_allocated(0), num_vertex_updated(0), primitive_indices(NULL), bvs(NULL), num_bvs(0) { } /// @brief copy from another BVH BVHModel(const BVHModel& other); /// @brief deconstruction, delete mesh data related. ~BVHModel() { delete [] vertices; delete [] tri_indices; delete [] bvs; delete [] prev_vertices; delete [] primitive_indices; } /// @brief We provide getBV() and getNumBVs() because BVH may be compressed (in future), so we must provide some flexibility here /// @brief Access the bv giving the its index const BVNode& getBV(int id) const { return bvs[id]; } /// @brief Access the bv giving the its index BVNode& getBV(int id) { return bvs[id]; } /// @brief Get the number of bv in the BVH int getNumBVs() const { return num_bvs; } /// @brief Get the object type: it is a BVH OBJECT_TYPE getObjectType() const { return OT_BVH; } /// @brief Get the BV type: default is unknown NODE_TYPE getNodeType() const { return BV_UNKNOWN; } /// @brief Compute the AABB for the BVH, used for broad-phase collision void computeLocalAABB(); /// @brief Begin a new BVH model int beginModel(int num_tris = 0, int num_vertices = 0); /// @brief Add one point in the new BVH model int addVertex(const Vec3f& p); /// @brief Add one triangle in the new BVH model int addTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3); /// @brief Add a set of triangles in the new BVH model int addSubModel(const std::vector& ps, const std::vector& ts); /// @brief Add a set of points in the new BVH model int addSubModel(const std::vector& ps); /// @brief End BVH model construction, will build the bounding volume hierarchy int endModel(); /// @brief Replace the geometry information of current frame (i.e. should have the same mesh topology with the previous frame) int beginReplaceModel(); /// @brief Replace one point in the old BVH model int replaceVertex(const Vec3f& p); /// @brief Replace one triangle in the old BVH model int replaceTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3); /// @brief Replace a set of points in the old BVH model int replaceSubModel(const std::vector& ps); /// @brief End BVH model replacement, will also refit or rebuild the bounding volume hierarchy int endReplaceModel(bool refit = true, bool bottomup = true); /// @brief Replace the geometry information of current frame (i.e. should have the same mesh topology with the previous frame). /// The current frame will be saved as the previous frame in prev_vertices. int beginUpdateModel(); /// @brief Update one point in the old BVH model int updateVertex(const Vec3f& p); /// @brief Update one triangle in the old BVH model int updateTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3); /// @brief Update a set of points in the old BVH model int updateSubModel(const std::vector& ps); /// @brief End BVH model update, will also refit or rebuild the bounding volume hierarchy int endUpdateModel(bool refit = true, bool bottomup = true); /// @brief Check the number of memory used int memUsage(int msg) const; /// @brief This is a special acceleration: BVH_model default stores the BV's transform in world coordinate. However, we can also store each BV's transform related to its parent /// BV node. When traversing the BVH, this can save one matrix transformation. void makeParentRelative() { Vec3f I[3] = {Vec3f(1, 0, 0), Vec3f(0, 1, 0), Vec3f(0, 0, 1)}; makeParentRelativeRecurse(0, I, Vec3f()); } Vec3f computeCOM() const { FCL_REAL vol = 0; Vec3f com; for(int i = 0; i < num_tris; ++i) { const Triangle& tri = tri_indices[i]; FCL_REAL d_six_vol = (vertices[tri[0]].cross(vertices[tri[1]])).dot(vertices[tri[2]]); vol += d_six_vol; com += (vertices[tri[0]] + vertices[tri[1]] + vertices[tri[2]]) * d_six_vol; } return com / (vol * 4); } FCL_REAL computeVolume() const { FCL_REAL vol = 0; for(int i = 0; i < num_tris; ++i) { const Triangle& tri = tri_indices[i]; FCL_REAL d_six_vol = (vertices[tri[0]].cross(vertices[tri[1]])).dot(vertices[tri[2]]); vol += d_six_vol; } return vol / 6; } Matrix3f computeMomentofInertia() const { Matrix3f C(0, 0, 0, 0, 0, 0, 0, 0, 0); Matrix3f C_canonical(1/60.0, 1/120.0, 1/120.0, 1/120.0, 1/60.0, 1/120.0, 1/120.0, 1/120.0, 1/60.0); for(int i = 0; i < num_tris; ++i) { const Triangle& tri = tri_indices[i]; const Vec3f& v1 = vertices[tri[0]]; const Vec3f& v2 = vertices[tri[1]]; const Vec3f& v3 = vertices[tri[2]]; FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3); Matrix3f A(v1, v2, v3); C += transpose(A) * C_canonical * A * d_six_vol; } FCL_REAL trace_C = C(0, 0) + C(1, 1) + C(2, 2); return Matrix3f(trace_C - C(0, 0), -C(0, 1), -C(0, 2), -C(1, 0), trace_C - C(1, 1), -C(1, 2), -C(2, 0), -C(2, 1), trace_C - C(2, 2)); } public: /// @brief Geometry point data Vec3f* vertices; /// @brief Geometry triangle index data, will be NULL for point clouds Triangle* tri_indices; /// @brief Geometry point data in previous frame Vec3f* prev_vertices; /// @brief Number of triangles int num_tris; /// @brief Number of points int num_vertices; /// @brief The state of BVH building process BVHBuildState build_state; /// @brief Split rule to split one BV node into two children std::shared_ptr > bv_splitter; /// @brief Fitting rule to fit a BV node to a set of geometry primitives std::shared_ptr > bv_fitter; private: int num_tris_allocated; int num_vertices_allocated; int num_bvs_allocated; int num_vertex_updated; /// for ccd vertex update unsigned int* primitive_indices; /// @brief Bounding volume hierarchy BVNode* bvs; /// @brief Number of BV nodes in bounding volume hierarchy int num_bvs; /// @brief Build the bounding volume hierarchy int buildTree(); /// @brief Refit the bounding volume hierarchy int refitTree(bool bottomup); /// @brief Refit the bounding volume hierarchy in a top-down way (slow but more compact) int refitTree_topdown(); /// @brief Refit the bounding volume hierarchy in a bottom-up way (fast but less compact) int refitTree_bottomup(); /// @brief Recursive kernel for hierarchy construction int recursiveBuildTree(int bv_id, int first_primitive, int num_primitives); /// @brief Recursive kernel for bottomup refitting int recursiveRefitTree_bottomup(int bv_id); /// @recursively compute each bv's transform related to its parent. For default BV, only the translation works. /// For oriented BV (OBB, RSS, OBBRSS), special implementation is provided. void makeParentRelativeRecurse(int bv_id, Vec3f parent_axis[], const Vec3f& parent_c) { if(!bvs[bv_id].isLeaf()) { makeParentRelativeRecurse(bvs[bv_id].first_child, parent_axis, bvs[bv_id].getCenter()); makeParentRelativeRecurse(bvs[bv_id].first_child + 1, parent_axis, bvs[bv_id].getCenter()); } bvs[bv_id].bv = translate(bvs[bv_id].bv, -parent_c); } }; template<> void BVHModel::makeParentRelativeRecurse(int bv_id, Vec3f parent_axis[], const Vec3f& parent_c); template<> void BVHModel::makeParentRelativeRecurse(int bv_id, Vec3f parent_axis[], const Vec3f& parent_c); template<> void BVHModel::makeParentRelativeRecurse(int bv_id, Vec3f parent_axis[], const Vec3f& parent_c); /// @brief Specialization of getNodeType() for BVHModel with different BV types template<> NODE_TYPE BVHModel::getNodeType() const; template<> NODE_TYPE BVHModel::getNodeType() const; template<> NODE_TYPE BVHModel::getNodeType() const; template<> NODE_TYPE BVHModel::getNodeType() const; template<> NODE_TYPE BVHModel::getNodeType() const; template<> NODE_TYPE BVHModel >::getNodeType() const; template<> NODE_TYPE BVHModel >::getNodeType() const; template<> NODE_TYPE BVHModel >::getNodeType() const; } #endif fcl-0.5.0/include/fcl/BVH/BVH_utility.h000066400000000000000000000100211274356571000174450ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_BVH_UTILITY_H #define FCL_BVH_UTILITY_H #include "fcl/BVH/BVH_model.h" namespace fcl { /// @brief Expand the BVH bounding boxes according to the variance matrix corresponding to the data stored within each BV node template void BVHExpand(BVHModel& model, const Variance3f* ucs, FCL_REAL r) { for(int i = 0; i < model.num_bvs; ++i) { BVNode& bvnode = model.getBV(i); BV bv; for(int j = 0; j < bvnode.num_primitives; ++j) { int v_id = bvnode.first_primitive + j; const Variance3f& uc = ucs[v_id]; Vec3f& v = model.vertices[bvnode.first_primitive + j]; for(int k = 0; k < 3; ++k) { bv += (v + uc.axis[k] * (r * uc.sigma[k])); bv += (v - uc.axis[k] * (r * uc.sigma[k])); } } bvnode.bv = bv; } } /// @brief Expand the BVH bounding boxes according to the corresponding variance information, for OBB void BVHExpand(BVHModel& model, const Variance3f* ucs, FCL_REAL r); /// @brief Expand the BVH bounding boxes according to the corresponding variance information, for RSS void BVHExpand(BVHModel& model, const Variance3f* ucs, FCL_REAL r); /// @brief Compute the covariance matrix for a set or subset of points. if ts = null, then indices refer to points directly; otherwise refer to triangles void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Matrix3f& M); /// @brief Compute the RSS bounding volume parameters: radius, rectangle size and the origin, given the BV axises. void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f axis[3], Vec3f& origin, FCL_REAL l[2], FCL_REAL& r); /// @brief Compute the bounding volume extent and center for a set or subset of points, given the BV axises. void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f axis[3], Vec3f& center, Vec3f& extent); /// @brief Compute the center and radius for a triangle's circumcircle void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c, Vec3f& center, FCL_REAL& radius); /// @brief Compute the maximum distance from a given center point to a point cloud FCL_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, const Vec3f& query); } #endif fcl-0.5.0/include/fcl/BVH/BV_fitter.h000066400000000000000000000236171274356571000171460ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_BV_FITTER_H #define FCL_BV_FITTER_H #include "fcl/BVH/BVH_internal.h" #include "fcl/BV/kIOS.h" #include "fcl/BV/OBBRSS.h" #include namespace fcl { /// @brief Compute a bounding volume that fits a set of n points. template void fit(Vec3f* ps, int n, BV& bv) { for(int i = 0; i < n; ++i) { bv += ps[i]; } } template<> void fit(Vec3f* ps, int n, OBB& bv); template<> void fit(Vec3f* ps, int n, RSS& bv); template<> void fit(Vec3f* ps, int n, kIOS& bv); template<> void fit(Vec3f* ps, int n, OBBRSS& bv); /// @brief Interface for fitting a bv given the triangles or points inside it. template class BVFitterBase { public: /// @brief Set the primitives to be processed by the fitter virtual void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_) = 0; /// @brief Set the primitives to be processed by the fitter, for deformable mesh. virtual void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_, BVHModelType type_) = 0; /// @brief Compute the fitting BV virtual BV fit(unsigned int* primitive_indices, int num_primitives) = 0; /// @brief clear the temporary data generated. virtual void clear() = 0; }; /// @brief The class for the default algorithm fitting a bounding volume to a set of points template class BVFitter : public BVFitterBase { public: /// @brief default deconstructor virtual ~BVFitter() {} /// @brief Prepare the geometry primitive data for fitting void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_) { vertices = vertices_; prev_vertices = NULL; tri_indices = tri_indices_; type = type_; } /// @brief Prepare the geometry primitive data for fitting, for deformable mesh void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_, BVHModelType type_) { vertices = vertices_; prev_vertices = prev_vertices_; tri_indices = tri_indices_; type = type_; } /// @brief Compute a bounding volume that fits a set of primitives (points or triangles). /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data BV fit(unsigned int* primitive_indices, int num_primitives) { BV bv; if(type == BVH_MODEL_TRIANGLES) /// The primitive is triangle { for(int i = 0; i < num_primitives; ++i) { Triangle t = tri_indices[primitive_indices[i]]; bv += vertices[t[0]]; bv += vertices[t[1]]; bv += vertices[t[2]]; if(prev_vertices) /// can fitting both current and previous frame { bv += prev_vertices[t[0]]; bv += prev_vertices[t[1]]; bv += prev_vertices[t[2]]; } } } else if(type == BVH_MODEL_POINTCLOUD) /// The primitive is point { for(int i = 0; i < num_primitives; ++i) { bv += vertices[primitive_indices[i]]; if(prev_vertices) /// can fitting both current and previous frame { bv += prev_vertices[primitive_indices[i]]; } } } return bv; } /// @brief Clear the geometry primitive data void clear() { vertices = NULL; prev_vertices = NULL; tri_indices = NULL; type = BVH_MODEL_UNKNOWN; } private: Vec3f* vertices; Vec3f* prev_vertices; Triangle* tri_indices; BVHModelType type; }; /// @brief Specification of BVFitter for OBB bounding volume template<> class BVFitter : public BVFitterBase { public: /// @brief Prepare the geometry primitive data for fitting void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_) { vertices = vertices_; prev_vertices = NULL; tri_indices = tri_indices_; type = type_; } /// @brief Prepare the geometry primitive data for fitting, for deformable mesh void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_, BVHModelType type_) { vertices = vertices_; prev_vertices = prev_vertices_; tri_indices = tri_indices_; type = type_; } /// @brief Compute a bounding volume that fits a set of primitives (points or triangles). /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data. OBB fit(unsigned int* primitive_indices, int num_primitives); /// brief Clear the geometry primitive data void clear() { vertices = NULL; prev_vertices = NULL; tri_indices = NULL; type = BVH_MODEL_UNKNOWN; } private: Vec3f* vertices; Vec3f* prev_vertices; Triangle* tri_indices; BVHModelType type; }; /// @brief Specification of BVFitter for RSS bounding volume template<> class BVFitter : public BVFitterBase { public: /// brief Prepare the geometry primitive data for fitting void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_) { vertices = vertices_; prev_vertices = NULL; tri_indices = tri_indices_; type = type_; } /// @brief Prepare the geometry primitive data for fitting, for deformable mesh void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_, BVHModelType type_) { vertices = vertices_; prev_vertices = prev_vertices_; tri_indices = tri_indices_; type = type_; } /// @brief Compute a bounding volume that fits a set of primitives (points or triangles). /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data. RSS fit(unsigned int* primitive_indices, int num_primitives); /// @brief Clear the geometry primitive data void clear() { vertices = NULL; prev_vertices = NULL; tri_indices = NULL; type = BVH_MODEL_UNKNOWN; } private: Vec3f* vertices; Vec3f* prev_vertices; Triangle* tri_indices; BVHModelType type; }; /// @brief Specification of BVFitter for kIOS bounding volume template<> class BVFitter : public BVFitterBase { public: /// @brief Prepare the geometry primitive data for fitting void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_) { vertices = vertices_; prev_vertices = NULL; tri_indices = tri_indices_; type = type_; } /// @brief Prepare the geometry primitive data for fitting void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_, BVHModelType type_) { vertices = vertices_; prev_vertices = prev_vertices_; tri_indices = tri_indices_; type = type_; } /// @brief Compute a bounding volume that fits a set of primitives (points or triangles). /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data. kIOS fit(unsigned int* primitive_indices, int num_primitives); /// @brief Clear the geometry primitive data void clear() { vertices = NULL; prev_vertices = NULL; tri_indices = NULL; type = BVH_MODEL_UNKNOWN; } private: Vec3f* vertices; Vec3f* prev_vertices; Triangle* tri_indices; BVHModelType type; }; /// @brief Specification of BVFitter for OBBRSS bounding volume template<> class BVFitter : public BVFitterBase { public: /// @brief Prepare the geometry primitive data for fitting void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_) { vertices = vertices_; prev_vertices = NULL; tri_indices = tri_indices_; type = type_; } /// @brief Prepare the geometry primitive data for fitting void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_, BVHModelType type_) { vertices = vertices_; prev_vertices = prev_vertices_; tri_indices = tri_indices_; type = type_; } /// @brief Compute a bounding volume that fits a set of primitives (points or triangles). /// The primitive data was set by set function and primitive_indices is the primitive index relative to the data. OBBRSS fit(unsigned int* primitive_indices, int num_primitives); /// @brief Clear the geometry primitive data void clear() { vertices = NULL; prev_vertices = NULL; tri_indices = NULL; type = BVH_MODEL_UNKNOWN; } private: Vec3f* vertices; Vec3f* prev_vertices; Triangle* tri_indices; BVHModelType type; }; } #endif fcl-0.5.0/include/fcl/BVH/BV_splitter.h000066400000000000000000000220451274356571000175110ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_BV_SPLITTER_H #define FCL_BV_SPLITTER_H #include "fcl/BVH/BVH_internal.h" #include "fcl/BV/kIOS.h" #include "fcl/BV/OBBRSS.h" #include #include namespace fcl { /// @brief Base interface for BV splitting algorithm template class BVSplitterBase { public: /// @brief Set the geometry data needed by the split rule virtual void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_) = 0; /// @brief Compute the split rule according to a subset of geometry and the corresponding BV node virtual void computeRule(const BV& bv, unsigned int* primitive_indices, int num_primitives) = 0; /// @brief Apply the split rule on a given point virtual bool apply(const Vec3f& q) const = 0; /// @brief Clear the geometry data set before virtual void clear() = 0; }; /// @brief Three types of split algorithms are provided in FCL as default enum SplitMethodType {SPLIT_METHOD_MEAN, SPLIT_METHOD_MEDIAN, SPLIT_METHOD_BV_CENTER}; /// @brief A class describing the split rule that splits each BV node template class BVSplitter : public BVSplitterBase { public: BVSplitter(SplitMethodType method) : split_method(method) { } /// @brief Default deconstructor virtual ~BVSplitter() {} /// @brief Set the geometry data needed by the split rule void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_) { vertices = vertices_; tri_indices = tri_indices_; type = type_; } /// @brief Compute the split rule according to a subset of geometry and the corresponding BV node void computeRule(const BV& bv, unsigned int* primitive_indices, int num_primitives) { switch(split_method) { case SPLIT_METHOD_MEAN: computeRule_mean(bv, primitive_indices, num_primitives); break; case SPLIT_METHOD_MEDIAN: computeRule_median(bv, primitive_indices, num_primitives); break; case SPLIT_METHOD_BV_CENTER: computeRule_bvcenter(bv, primitive_indices, num_primitives); break; default: std::cerr << "Split method not supported" << std::endl; } } /// @brief Apply the split rule on a given point bool apply(const Vec3f& q) const { return q[split_axis] > split_value; } /// @brief Clear the geometry data set before void clear() { vertices = NULL; tri_indices = NULL; type = BVH_MODEL_UNKNOWN; } private: /// @brief The axis based on which the split decision is made. For most BV, the axis is aligned with one of the world coordinate, so only split_axis is needed. /// For oriented node, we can use a vector to make a better split decision. int split_axis; Vec3f split_vector; /// @brief The split threshold, different primitives are splitted according whether their projection on the split_axis is larger or smaller than the threshold FCL_REAL split_value; /// @brief The mesh vertices or points handled by the splitter Vec3f* vertices; /// @brief The triangles handled by the splitter Triangle* tri_indices; /// @brief Whether the geometry is mesh or point cloud BVHModelType type; /// @brief The split algorithm used SplitMethodType split_method; /// @brief Split algorithm 1: Split the node from center void computeRule_bvcenter(const BV& bv, unsigned int* primitive_indices, int num_primitives) { Vec3f center = bv.center(); int axis = 2; if(bv.width() >= bv.height() && bv.width() >= bv.depth()) axis = 0; else if(bv.height() >= bv.width() && bv.height() >= bv.depth()) axis = 1; split_axis = axis; split_value = center[axis]; } /// @brief Split algorithm 2: Split the node according to the mean of the data contained void computeRule_mean(const BV& bv, unsigned int* primitive_indices, int num_primitives) { int axis = 2; if(bv.width() >= bv.height() && bv.width() >= bv.depth()) axis = 0; else if(bv.height() >= bv.width() && bv.height() >= bv.depth()) axis = 1; split_axis = axis; FCL_REAL sum = 0; if(type == BVH_MODEL_TRIANGLES) { for(int i = 0; i < num_primitives; ++i) { const Triangle& t = tri_indices[primitive_indices[i]]; sum += (vertices[t[0]][split_axis] + vertices[t[1]][split_axis] + vertices[t[2]][split_axis]); } sum /= 3; } else if(type == BVH_MODEL_POINTCLOUD) { for(int i = 0; i < num_primitives; ++i) { sum += vertices[primitive_indices[i]][split_axis]; } } split_value = sum / num_primitives; } /// @brief Split algorithm 3: Split the node according to the median of the data contained void computeRule_median(const BV& bv, unsigned int* primitive_indices, int num_primitives) { int axis = 2; if(bv.width() >= bv.height() && bv.width() >= bv.depth()) axis = 0; else if(bv.height() >= bv.width() && bv.height() >= bv.depth()) axis = 1; split_axis = axis; std::vector proj(num_primitives); if(type == BVH_MODEL_TRIANGLES) { for(int i = 0; i < num_primitives; ++i) { const Triangle& t = tri_indices[primitive_indices[i]]; proj[i] = (vertices[t[0]][split_axis] + vertices[t[1]][split_axis] + vertices[t[2]][split_axis]) / 3; } } else if(type == BVH_MODEL_POINTCLOUD) { for(int i = 0; i < num_primitives; ++i) proj[i] = vertices[primitive_indices[i]][split_axis]; } std::sort(proj.begin(), proj.end()); if(num_primitives % 2 == 1) { split_value = proj[(num_primitives - 1) / 2]; } else { split_value = (proj[num_primitives / 2] + proj[num_primitives / 2 - 1]) / 2; } } }; template<> bool BVSplitter::apply(const Vec3f& q) const; template<> bool BVSplitter::apply(const Vec3f& q) const; template<> bool BVSplitter::apply(const Vec3f& q) const; template<> bool BVSplitter::apply(const Vec3f& q) const; template<> void BVSplitter::computeRule_bvcenter(const OBB& bv, unsigned int* primitive_indices, int num_primitives); template<> void BVSplitter::computeRule_mean(const OBB& bv, unsigned int* primitive_indices, int num_primitives); template<> void BVSplitter::computeRule_median(const OBB& bv, unsigned int* primitive_indices, int num_primitives); template<> void BVSplitter::computeRule_bvcenter(const RSS& bv, unsigned int* primitive_indices, int num_primitives); template<> void BVSplitter::computeRule_mean(const RSS& bv, unsigned int* primitive_indices, int num_primitives); template<> void BVSplitter::computeRule_median(const RSS& bv, unsigned int* primitive_indices, int num_primitives); template<> void BVSplitter::computeRule_bvcenter(const kIOS& bv, unsigned int* primitive_indices, int num_primitives); template<> void BVSplitter::computeRule_mean(const kIOS& bv, unsigned int* primitive_indices, int num_primitives); template<> void BVSplitter::computeRule_median(const kIOS& bv, unsigned int* primitive_indices, int num_primitives); template<> void BVSplitter::computeRule_bvcenter(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives); template<> void BVSplitter::computeRule_mean(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives); template<> void BVSplitter::computeRule_median(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives); } #endif fcl-0.5.0/include/fcl/CMakeLists.txt000066400000000000000000000011131274356571000172150ustar00rootroot00000000000000file(GLOB_RECURSE HEADERS ${CMAKE_CURRENT_SOURCE_DIR}/*.h ${CMAKE_CURRENT_SOURCE_DIR}/*.hxx) file(GLOB_RECURSE CONFIGURED_HEADERS ${CMAKE_CURRENT_BINARY_DIR}/*.h ${CMAKE_CURRENT_BINARY_DIR}/*.hxx) set(FCL_HEADERS ${HEADERS} ${CONFIGURED_HEADERS} PARENT_SCOPE) file(TO_NATIVE_PATH "${CMAKE_CURRENT_SOURCE_DIR}" FCL_CONFIG_IN_DIR) file(TO_NATIVE_PATH "${CMAKE_CURRENT_BINARY_DIR}" FCL_CONFIG_OUT_DIR) configure_file("${FCL_CONFIG_IN_DIR}/config.h.in" "${FCL_CONFIG_OUT_DIR}/config.h") install(FILES "${FCL_CONFIG_OUT_DIR}/config.h" DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/fcl) fcl-0.5.0/include/fcl/articulated_model/000077500000000000000000000000001274356571000201425ustar00rootroot00000000000000fcl-0.5.0/include/fcl/articulated_model/joint.h000066400000000000000000000110251274356571000214350ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Dalibor Matura, Jia Pan */ #ifndef FCL_ARTICULATED_MODEL_JOINT_H #define FCL_ARTICULATED_MODEL_JOINT_H #include "fcl/math/transform.h" #include "fcl/data_types.h" #include #include #include #include #include namespace fcl { class JointConfig; class Link; enum JointType {JT_UNKNOWN, JT_PRISMATIC, JT_REVOLUTE, JT_BALLEULER}; /// @brief Base Joint class Joint { public: Joint(const std::shared_ptr& link_parent, const std::shared_ptr& link_child, const Transform3f& transform_to_parent, const std::string& name); Joint(const std::string& name); virtual ~Joint() {} const std::string& getName() const; void setName(const std::string& name); virtual Transform3f getLocalTransform() const = 0; virtual std::size_t getNumDofs() const = 0; std::shared_ptr getJointConfig() const; void setJointConfig(const std::shared_ptr& joint_cfg); std::shared_ptr getParentLink() const; std::shared_ptr getChildLink() const; void setParentLink(const std::shared_ptr& link); void setChildLink(const std::shared_ptr& link); JointType getJointType() const; const Transform3f& getTransformToParent() const; void setTransformToParent(const Transform3f& t); protected: /// links to parent and child are only for connection, so weak_ptr to avoid cyclic dependency std::weak_ptr link_parent_, link_child_; JointType type_; std::string name_; std::shared_ptr joint_cfg_; Transform3f transform_to_parent_; }; class PrismaticJoint : public Joint { public: PrismaticJoint(const std::shared_ptr& link_parent, const std::shared_ptr& link_child, const Transform3f& transform_to_parent, const std::string& name, const Vec3f& axis); virtual ~PrismaticJoint() {} Transform3f getLocalTransform() const; std::size_t getNumDofs() const; const Vec3f& getAxis() const; protected: Vec3f axis_; }; class RevoluteJoint : public Joint { public: RevoluteJoint(const std::shared_ptr& link_parent, const std::shared_ptr& link_child, const Transform3f& transform_to_parent, const std::string& name, const Vec3f& axis); virtual ~RevoluteJoint() {} Transform3f getLocalTransform() const; std::size_t getNumDofs() const; const Vec3f& getAxis() const; protected: Vec3f axis_; }; class BallEulerJoint : public Joint { public: BallEulerJoint(const std::shared_ptr& link_parent, const std::shared_ptr& link_child, const Transform3f& transform_to_parent, const std::string& name); virtual ~BallEulerJoint() {} std::size_t getNumDofs() const; Transform3f getLocalTransform() const; }; } #endif fcl-0.5.0/include/fcl/articulated_model/joint_config.h000066400000000000000000000055521274356571000227720ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Dalibor Matura, Jia Pan */ #ifndef FCL_ARTICULATED_MODEL_JOINT_CONFIG_H #define FCL_ARTICULATED_MODEL_JOINT_CONFIG_H #include "fcl/data_types.h" #include #include namespace fcl { class Joint; class JointConfig { public: JointConfig(); JointConfig(const JointConfig& joint_cfg); JointConfig(const std::shared_ptr& joint, FCL_REAL default_value = 0, FCL_REAL default_value_min = 0, FCL_REAL default_value_max = 0); std::size_t getDim() const; inline FCL_REAL operator [] (std::size_t i) const { return values_[i]; } inline FCL_REAL& operator [] (std::size_t i) { return values_[i]; } FCL_REAL getValue(std::size_t i) const; FCL_REAL& getValue(std::size_t i); FCL_REAL getLimitMin(std::size_t i) const; FCL_REAL& getLimitMin(std::size_t i); FCL_REAL getLimitMax(std::size_t i) const; FCL_REAL& getLimitMax(std::size_t i); std::shared_ptr getJoint() const; private: std::weak_ptr joint_; std::vector values_; std::vector limits_min_; std::vector limits_max_; }; } #endif fcl-0.5.0/include/fcl/articulated_model/link.h000066400000000000000000000050701274356571000212520ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Dalibor Matura, Jia Pan */ #ifndef FCL_ARTICULATED_MODEL_LINK_H #define FCL_ARTICULATED_MODEL_LINK_H #include "fcl/data_types.h" #include "fcl/collision_object.h" #include #include namespace fcl { class Joint; class Link { public: Link(const std::string& name); const std::string& getName() const; void setName(const std::string& name); void addChildJoint(const std::shared_ptr& joint); void setParentJoint(const std::shared_ptr& joint); void addObject(const std::shared_ptr& object); std::size_t getNumChildJoints() const; std::size_t getNumObjects() const; protected: std::string name_; std::vector > objects_; std::vector > children_joints_; std::shared_ptr parent_joint_; }; } #endif fcl-0.5.0/include/fcl/articulated_model/model.h000066400000000000000000000061151274356571000214160ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Dalibor Matura, Jia Pan */ #ifndef FCL_ARTICULATED_MODEL_MODEL_H #define FCL_ARTICULATED_MODEL_MODEL_H #include "fcl/articulated_model/joint.h" #include "fcl/articulated_model/link.h" #include "fcl/data_types.h" #include #include #include namespace fcl { class ModelParseError : public std::runtime_error { public: ModelParseError(const std::string& error_msg) : std::runtime_error(error_msg) {} }; class Model { public: Model() {} virtual ~Model() {} const std::string& getName() const; void addLink(const std::shared_ptr& link); void addJoint(const std::shared_ptr& joint); void initRoot(const std::map& link_parent_tree); void initTree(std::map& link_parent_tree); std::size_t getNumDofs() const; std::size_t getNumLinks() const; std::size_t getNumJoints() const; std::shared_ptr getRoot() const; std::shared_ptr getLink(const std::string& name) const; std::shared_ptr getJoint(const std::string& name) const; std::vector > getLinks() const; std::vector > getJoints() const; protected: std::shared_ptr root_link_; std::map > links_; std::map > joints_; std::string name_; }; } #endif fcl-0.5.0/include/fcl/articulated_model/model_config.h000066400000000000000000000051361274356571000227450ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Dalibor Matura, Jia Pan */ #ifndef FCL_ARTICULATED_MODEL_MODEL_CONFIG_H #define FCL_ARTICULATED_MODEL_MODEL_CONFIG_H #include "fcl/data_types.h" #include "fcl/articulated_model/joint_config.h" #include #include #include namespace fcl { class ModelConfig { public: ModelConfig(); ModelConfig(const ModelConfig& model_cfg); ModelConfig(std::map > joints_map); JointConfig getJointConfigByJointName(const std::string& joint_name) const; JointConfig& getJointConfigByJointName(const std::string& joint_name); JointConfig getJointConfigByJoint(std::shared_ptr joint) const; JointConfig& getJointConfigByJoint(std::shared_ptr joint); std::map getJointCfgsMap() const { return joint_cfgs_map_; } private: std::map joint_cfgs_map_; }; } #endif fcl-0.5.0/include/fcl/broadphase/000077500000000000000000000000001274356571000165715ustar00rootroot00000000000000fcl-0.5.0/include/fcl/broadphase/broadphase.h000066400000000000000000000227041274356571000210570ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_BROAD_PHASE_H #define FCL_BROAD_PHASE_H #include "fcl/collision_object.h" #include #include namespace fcl { /// @brief Callback for collision between two objects. Return value is whether can stop now. typedef bool (*CollisionCallBack)(CollisionObject* o1, CollisionObject* o2, void* cdata); /// @brief Callback for distance between two objects, Return value is whether can stop now, also return the minimum distance till now. typedef bool (*DistanceCallBack)(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& dist); /// @brief Base class for broad phase collision. It helps to accelerate the collision/distance between N objects. Also support self collision, self distance and collision/distance with another M objects. class BroadPhaseCollisionManager { public: BroadPhaseCollisionManager() : enable_tested_set_(false) { } virtual ~BroadPhaseCollisionManager() {} /// @brief add objects to the manager virtual void registerObjects(const std::vector& other_objs) { for(size_t i = 0; i < other_objs.size(); ++i) registerObject(other_objs[i]); } /// @brief add one object to the manager virtual void registerObject(CollisionObject* obj) = 0; /// @brief remove one object from the manager virtual void unregisterObject(CollisionObject* obj) = 0; /// @brief initialize the manager, related with the specific type of manager virtual void setup() = 0; /// @brief update the condition of manager virtual void update() = 0; /// @brief update the manager by explicitly given the object updated virtual void update(CollisionObject* updated_obj) { update(); } /// @brief update the manager by explicitly given the set of objects update virtual void update(const std::vector& updated_objs) { update(); } /// @brief clear the manager virtual void clear() = 0; /// @brief return the objects managed by the manager virtual void getObjects(std::vector& objs) const = 0; /// @brief perform collision test between one object and all the objects belonging to the manager virtual void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const = 0; /// @brief perform distance computation between one object and all the objects belonging to the manager virtual void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const = 0; /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) virtual void collide(void* cdata, CollisionCallBack callback) const = 0; /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) virtual void distance(void* cdata, DistanceCallBack callback) const = 0; /// @brief perform collision test with objects belonging to another manager virtual void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const = 0; /// @brief perform distance test with objects belonging to another manager virtual void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const = 0; /// @brief whether the manager is empty virtual bool empty() const = 0; /// @brief the number of objects managed by the manager virtual size_t size() const = 0; protected: /// @brief tools help to avoid repeating collision or distance callback for the pairs of objects tested before. It can be useful for some of the broadphase algorithms. mutable std::set > tested_set; mutable bool enable_tested_set_; bool inTestedSet(CollisionObject* a, CollisionObject* b) const { if(a < b) return tested_set.find(std::make_pair(a, b)) != tested_set.end(); else return tested_set.find(std::make_pair(b, a)) != tested_set.end(); } void insertTestedSet(CollisionObject* a, CollisionObject* b) const { if(a < b) tested_set.insert(std::make_pair(a, b)); else tested_set.insert(std::make_pair(b, a)); } }; /// @brief Callback for continuous collision between two objects. Return value is whether can stop now. typedef bool (*ContinuousCollisionCallBack)(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata); /// @brief Callback for continuous distance between two objects, Return value is whether can stop now, also return the minimum distance till now. typedef bool (*ContinuousDistanceCallBack)(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata, FCL_REAL& dist); /// @brief Base class for broad phase continuous collision. It helps to accelerate the continuous collision/distance between N objects. Also support self collision, self distance and collision/distance with another M objects. class BroadPhaseContinuousCollisionManager { public: BroadPhaseContinuousCollisionManager() { } virtual ~BroadPhaseContinuousCollisionManager() {} /// @brief add objects to the manager virtual void registerObjects(const std::vector& other_objs) { for(size_t i = 0; i < other_objs.size(); ++i) registerObject(other_objs[i]); } /// @brief add one object to the manager virtual void registerObject(ContinuousCollisionObject* obj) = 0; /// @brief remove one object from the manager virtual void unregisterObject(ContinuousCollisionObject* obj) = 0; /// @brief initialize the manager, related with the specific type of manager virtual void setup() = 0; /// @brief update the condition of manager virtual void update() = 0; /// @brief update the manager by explicitly given the object updated virtual void update(ContinuousCollisionObject* updated_obj) { update(); } /// @brief update the manager by explicitly given the set of objects update virtual void update(const std::vector& updated_objs) { update(); } /// @brief clear the manager virtual void clear() = 0; /// @brief return the objects managed by the manager virtual void getObjects(std::vector& objs) const = 0; /// @brief perform collision test between one object and all the objects belonging to the manager virtual void collide(ContinuousCollisionObject* obj, void* cdata, CollisionCallBack callback) const = 0; /// @brief perform distance computation between one object and all the objects belonging to the manager virtual void distance(ContinuousCollisionObject* obj, void* cdata, DistanceCallBack callback) const = 0; /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) virtual void collide(void* cdata, CollisionCallBack callback) const = 0; /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) virtual void distance(void* cdata, DistanceCallBack callback) const = 0; /// @brief perform collision test with objects belonging to another manager virtual void collide(BroadPhaseContinuousCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const = 0; /// @brief perform distance test with objects belonging to another manager virtual void distance(BroadPhaseContinuousCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const = 0; /// @brief whether the manager is empty virtual bool empty() const = 0; /// @brief the number of objects managed by the manager virtual size_t size() const = 0; }; } #include "fcl/broadphase/broadphase_bruteforce.h" #include "fcl/broadphase/broadphase_spatialhash.h" #include "fcl/broadphase/broadphase_SaP.h" #include "fcl/broadphase/broadphase_SSaP.h" #include "fcl/broadphase/broadphase_interval_tree.h" #include "fcl/broadphase/broadphase_dynamic_AABB_tree.h" #include "fcl/broadphase/broadphase_dynamic_AABB_tree_array.h" #endif fcl-0.5.0/include/fcl/broadphase/broadphase_SSaP.h000066400000000000000000000141451274356571000217450ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_BROAD_PHASE_SSAP_H #define FCL_BROAD_PHASE_SSAP_H #include "fcl/broadphase/broadphase.h" namespace fcl { /// @brief Simple SAP collision manager class SSaPCollisionManager : public BroadPhaseCollisionManager { public: SSaPCollisionManager() : setup_(false) {} /// @brief remove one object from the manager void registerObject(CollisionObject* obj); /// @brief add one object to the manager void unregisterObject(CollisionObject* obj); /// @brief initialize the manager, related with the specific type of manager void setup(); /// @brief update the condition of manager void update(); /// @brief clear the manager void clear(); /// @brief return the objects managed by the manager void getObjects(std::vector& objs) const; /// @brief perform collision test between one object and all the objects belonging to the manager void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; /// @brief perform distance computation between one object and all the objects belonging to the manager void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const; /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) void collide(void* cdata, CollisionCallBack callback) const; /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) void distance(void* cdata, DistanceCallBack callback) const; /// @brief perform collision test with objects belonging to another manager void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const; /// @brief perform distance test with objects belonging to another manager void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const; /// @brief whether the manager is empty bool empty() const; /// @brief the number of objects managed by the manager inline size_t size() const { return objs_x.size(); } protected: /// @brief check collision between one object and a list of objects, return value is whether stop is possible bool checkColl(std::vector::const_iterator pos_start, std::vector::const_iterator pos_end, CollisionObject* obj, void* cdata, CollisionCallBack callback) const; /// @brief check distance between one object and a list of objects, return value is whether stop is possible bool checkDis(std::vector::const_iterator pos_start, std::vector::const_iterator pos_end, CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const; bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const; static inline size_t selectOptimalAxis(const std::vector& objs_x, const std::vector& objs_y, const std::vector& objs_z, std::vector::const_iterator& it_beg, std::vector::const_iterator& it_end) { /// simple sweep and prune method double delta_x = (objs_x[objs_x.size() - 1])->getAABB().min_[0] - (objs_x[0])->getAABB().min_[0]; double delta_y = (objs_x[objs_y.size() - 1])->getAABB().min_[1] - (objs_y[0])->getAABB().min_[1]; double delta_z = (objs_z[objs_z.size() - 1])->getAABB().min_[2] - (objs_z[0])->getAABB().min_[2]; int axis = 0; if(delta_y > delta_x && delta_y > delta_z) axis = 1; else if(delta_z > delta_y && delta_z > delta_x) axis = 2; switch(axis) { case 0: it_beg = objs_x.begin(); it_end = objs_x.end(); break; case 1: it_beg = objs_y.begin(); it_end = objs_y.end(); break; case 2: it_beg = objs_z.begin(); it_end = objs_z.end(); break; } return axis; } /// @brief Objects sorted according to lower x value std::vector objs_x; /// @brief Objects sorted according to lower y value std::vector objs_y; /// @brief Objects sorted according to lower z value std::vector objs_z; /// @brief tag about whether the environment is maintained suitably (i.e., the objs_x, objs_y, objs_z are sorted correctly bool setup_; }; } #endif fcl-0.5.0/include/fcl/broadphase/broadphase_SaP.h000066400000000000000000000207421274356571000216220ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_BROAD_PHASE_SAP_H #define FCL_BROAD_PHASE_SAP_H #include "fcl/broadphase/broadphase.h" #include #include namespace fcl { /// @brief Rigorous SAP collision manager class SaPCollisionManager : public BroadPhaseCollisionManager { public: SaPCollisionManager() { elist[0] = NULL; elist[1] = NULL; elist[2] = NULL; optimal_axis = 0; } ~SaPCollisionManager() { clear(); } /// @brief add objects to the manager void registerObjects(const std::vector& other_objs); /// @brief remove one object from the manager void registerObject(CollisionObject* obj); /// @brief add one object to the manager void unregisterObject(CollisionObject* obj); /// @brief initialize the manager, related with the specific type of manager void setup(); /// @brief update the condition of manager void update(); /// @brief update the manager by explicitly given the object updated void update(CollisionObject* updated_obj); /// @brief update the manager by explicitly given the set of objects update void update(const std::vector& updated_objs); /// @brief clear the manager void clear(); /// @brief return the objects managed by the manager void getObjects(std::vector& objs) const; /// @brief perform collision test between one object and all the objects belonging to the manager void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; /// @brief perform distance computation between one object and all the objects belonging to the manager void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const; /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) void collide(void* cdata, CollisionCallBack callback) const; /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) void distance(void* cdata, DistanceCallBack callback) const; /// @brief perform collision test with objects belonging to another manager void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const; /// @brief perform distance test with objects belonging to another manager void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const; /// @brief whether the manager is empty bool empty() const; /// @brief the number of objects managed by the manager inline size_t size() const { return AABB_arr.size(); } protected: struct EndPoint; /// @brief SAP interval for one object struct SaPAABB { /// @brief object CollisionObject* obj; /// @brief lower bound end point of the interval EndPoint* lo; /// @brief higher bound end point of the interval EndPoint* hi; /// @brief cached AABB value AABB cached; }; /// @brief End point for an interval struct EndPoint { /// @brief tag for whether it is a lower bound or higher bound of an interval, 0 for lo, and 1 for hi char minmax; /// @brief back pointer to SAP interval SaPAABB* aabb; /// @brief the previous end point in the end point list EndPoint* prev[3]; /// @brief the next end point in the end point list EndPoint* next[3]; /// @brief get the value of the end point inline const Vec3f& getVal() const { if(minmax) return aabb->cached.max_; else return aabb->cached.min_; } /// @brief set the value of the end point inline Vec3f& getVal() { if(minmax) return aabb->cached.max_; else return aabb->cached.min_; } inline Vec3f::U getVal(size_t i) const { if(minmax) return aabb->cached.max_[i]; else return aabb->cached.min_[i]; } inline Vec3f::U& getVal(size_t i) { if(minmax) return aabb->cached.max_[i]; else return aabb->cached.min_[i]; } }; /// @brief A pair of objects that are not culling away and should further check collision struct SaPPair { SaPPair(CollisionObject* a, CollisionObject* b) { if(a < b) { obj1 = a; obj2 = b; } else { obj1 = b; obj2 = a; } } CollisionObject* obj1; CollisionObject* obj2; bool operator == (const SaPPair& other) const { return ((obj1 == other.obj1) && (obj2 == other.obj2)); } }; /// @brief Functor to help unregister one object class isUnregistered { CollisionObject* obj; public: isUnregistered(CollisionObject* obj_) : obj(obj_) {} bool operator() (const SaPPair& pair) const { return (pair.obj1 == obj) || (pair.obj2 == obj); } }; /// @brief Functor to help remove collision pairs no longer valid (i.e., should be culled away) class isNotValidPair { CollisionObject* obj1; CollisionObject* obj2; public: isNotValidPair(CollisionObject* obj1_, CollisionObject* obj2_) : obj1(obj1_), obj2(obj2_) {} bool operator() (const SaPPair& pair) { return (pair.obj1 == obj1) && (pair.obj2 == obj2); } }; void update_(SaPAABB* updated_aabb); void updateVelist() { for(int coord = 0; coord < 3; ++coord) { velist[coord].resize(size() * 2); EndPoint* current = elist[coord]; size_t id = 0; while(current) { velist[coord][id] = current; current = current->next[coord]; id++; } } } /// @brief End point list for x, y, z coordinates EndPoint* elist[3]; /// @brief vector version of elist, for acceleration std::vector velist[3]; /// @brief SAP interval list std::list AABB_arr; /// @brief The pair of objects that should further check for collision std::list overlap_pairs; size_t optimal_axis; std::map obj_aabb_map; bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const; bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; void addToOverlapPairs(const SaPPair& p) { bool repeated = false; for(std::list::iterator it = overlap_pairs.begin(), end = overlap_pairs.end(); it != end; ++it) { if(*it == p) { repeated = true; break; } } if(!repeated) overlap_pairs.push_back(p); } void removeFromOverlapPairs(const SaPPair& p) { for(std::list::iterator it = overlap_pairs.begin(), end = overlap_pairs.end(); it != end; ++it) { if(*it == p) { overlap_pairs.erase(it); break; } } // or overlap_pairs.remove_if(isNotValidPair(p)); } }; } #endif fcl-0.5.0/include/fcl/broadphase/broadphase_bruteforce.h000066400000000000000000000100031274356571000232640ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_BROAD_PHASE_BRUTE_FORCE_H #define FCL_BROAD_PHASE_BRUTE_FORCE_H #include "fcl/broadphase/broadphase.h" #include namespace fcl { /// @brief Brute force N-body collision manager class NaiveCollisionManager : public BroadPhaseCollisionManager { public: NaiveCollisionManager() {} /// @brief add objects to the manager void registerObjects(const std::vector& other_objs); /// @brief add one object to the manager void registerObject(CollisionObject* obj); /// @brief remove one object from the manager void unregisterObject(CollisionObject* obj); /// @brief initialize the manager, related with the specific type of manager void setup(); /// @brief update the condition of manager void update(); /// @brief clear the manager void clear(); /// @brief return the objects managed by the manager void getObjects(std::vector& objs) const; /// @brief perform collision test between one object and all the objects belonging to the manager void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; /// @brief perform distance computation between one object and all the objects belonging to the manager void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const; /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) void collide(void* cdata, CollisionCallBack callback) const; /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) void distance(void* cdata, DistanceCallBack callback) const; /// @brief perform collision test with objects belonging to another manager void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const; /// @brief perform distance test with objects belonging to another manager void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const; /// @brief whether the manager is empty bool empty() const; /// @brief the number of objects managed by the manager inline size_t size() const { return objs.size(); } protected: /// @brief objects belonging to the manager are stored in a list structure std::list objs; }; } #endif fcl-0.5.0/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h000066400000000000000000000130771274356571000243520ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_H #define FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_H #include "fcl/broadphase/broadphase.h" #include "fcl/broadphase/hierarchy_tree.h" #include "fcl/BV/BV.h" #include "fcl/shape/geometric_shapes_utility.h" #include #include #include namespace fcl { class DynamicAABBTreeCollisionManager : public BroadPhaseCollisionManager { public: typedef NodeBase DynamicAABBNode; typedef std::unordered_map DynamicAABBTable; int max_tree_nonbalanced_level; int tree_incremental_balance_pass; int& tree_topdown_balance_threshold; int& tree_topdown_level; int tree_init_level; bool octree_as_geometry_collide; bool octree_as_geometry_distance; DynamicAABBTreeCollisionManager() : tree_topdown_balance_threshold(dtree.bu_threshold), tree_topdown_level(dtree.topdown_level) { max_tree_nonbalanced_level = 10; tree_incremental_balance_pass = 10; tree_topdown_balance_threshold = 2; tree_topdown_level = 0; tree_init_level = 0; setup_ = false; // from experiment, this is the optimal setting octree_as_geometry_collide = true; octree_as_geometry_distance = false; } /// @brief add objects to the manager void registerObjects(const std::vector& other_objs); /// @brief add one object to the manager void registerObject(CollisionObject* obj); /// @brief remove one object from the manager void unregisterObject(CollisionObject* obj); /// @brief initialize the manager, related with the specific type of manager void setup(); /// @brief update the condition of manager void update(); /// @brief update the manager by explicitly given the object updated void update(CollisionObject* updated_obj); /// @brief update the manager by explicitly given the set of objects update void update(const std::vector& updated_objs); /// @brief clear the manager void clear() { dtree.clear(); table.clear(); } /// @brief return the objects managed by the manager void getObjects(std::vector& objs) const { objs.resize(this->size()); std::transform(table.begin(), table.end(), objs.begin(), std::bind(&DynamicAABBTable::value_type::first, std::placeholders::_1)); } /// @brief perform collision test between one object and all the objects belonging to the manager void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; /// @brief perform distance computation between one object and all the objects belonging to the manager void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const; /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) void collide(void* cdata, CollisionCallBack callback) const; /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) void distance(void* cdata, DistanceCallBack callback) const; /// @brief perform collision test with objects belonging to another manager void collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const; /// @brief perform distance test with objects belonging to another manager void distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const; /// @brief whether the manager is empty bool empty() const { return dtree.empty(); } /// @brief the number of objects managed by the manager size_t size() const { return dtree.size(); } const HierarchyTree& getTree() const { return dtree; } private: HierarchyTree dtree; std::unordered_map table; bool setup_; void update_(CollisionObject* updated_obj); }; } #endif fcl-0.5.0/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h000066400000000000000000000132121274356571000255370ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_H #define FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_H #include "fcl/broadphase/broadphase.h" #include "fcl/broadphase/hierarchy_tree.h" #include "fcl/BV/BV.h" #include "fcl/shape/geometric_shapes_utility.h" #include #include #include namespace fcl { class DynamicAABBTreeCollisionManager_Array : public BroadPhaseCollisionManager { public: typedef implementation_array::NodeBase DynamicAABBNode; typedef std::unordered_map DynamicAABBTable; int max_tree_nonbalanced_level; int tree_incremental_balance_pass; int& tree_topdown_balance_threshold; int& tree_topdown_level; int tree_init_level; bool octree_as_geometry_collide; bool octree_as_geometry_distance; DynamicAABBTreeCollisionManager_Array() : tree_topdown_balance_threshold(dtree.bu_threshold), tree_topdown_level(dtree.topdown_level) { max_tree_nonbalanced_level = 10; tree_incremental_balance_pass = 10; tree_topdown_balance_threshold = 2; tree_topdown_level = 0; tree_init_level = 0; setup_ = false; // from experiment, this is the optimal setting octree_as_geometry_collide = true; octree_as_geometry_distance = false; } /// @brief add objects to the manager void registerObjects(const std::vector& other_objs); /// @brief add one object to the manager void registerObject(CollisionObject* obj); /// @brief remove one object from the manager void unregisterObject(CollisionObject* obj); /// @brief initialize the manager, related with the specific type of manager void setup(); /// @brief update the condition of manager void update(); /// @brief update the manager by explicitly given the object updated void update(CollisionObject* updated_obj); /// @brief update the manager by explicitly given the set of objects update void update(const std::vector& updated_objs); /// @brief clear the manager void clear() { dtree.clear(); table.clear(); } /// @brief return the objects managed by the manager void getObjects(std::vector& objs) const { objs.resize(this->size()); std::transform(table.begin(), table.end(), objs.begin(), std::bind(&DynamicAABBTable::value_type::first, std::placeholders::_1)); } /// @brief perform collision test between one object and all the objects belonging to the manager void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; /// @brief perform distance computation between one object and all the objects belonging to the manager void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const; /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) void collide(void* cdata, CollisionCallBack callback) const; /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) void distance(void* cdata, DistanceCallBack callback) const; /// @brief perform collision test with objects belonging to another manager void collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const; /// @brief perform distance test with objects belonging to another manager void distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const; /// @brief whether the manager is empty bool empty() const { return dtree.empty(); } /// @brief the number of objects managed by the manager size_t size() const { return dtree.size(); } const implementation_array::HierarchyTree& getTree() const { return dtree; } private: implementation_array::HierarchyTree dtree; std::unordered_map table; bool setup_; void update_(CollisionObject* updated_obj); }; } #endif fcl-0.5.0/include/fcl/broadphase/broadphase_interval_tree.h000066400000000000000000000135751274356571000240100ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_BROAD_PHASE_INTERVAL_TREE_H #define FCL_BROAD_PHASE_INTERVAL_TREE_H #include "fcl/broadphase/broadphase.h" #include "fcl/broadphase/interval_tree.h" #include #include namespace fcl { /// @brief Collision manager based on interval tree class IntervalTreeCollisionManager : public BroadPhaseCollisionManager { public: IntervalTreeCollisionManager() : setup_(false) { for(int i = 0; i < 3; ++i) interval_trees[i] = NULL; } ~IntervalTreeCollisionManager() { clear(); } /// @brief remove one object from the manager void registerObject(CollisionObject* obj); /// @brief add one object to the manager void unregisterObject(CollisionObject* obj); /// @brief initialize the manager, related with the specific type of manager void setup(); /// @brief update the condition of manager void update(); /// @brief update the manager by explicitly given the object updated void update(CollisionObject* updated_obj); /// @brief update the manager by explicitly given the set of objects update void update(const std::vector& updated_objs); /// @brief clear the manager void clear(); /// @brief return the objects managed by the manager void getObjects(std::vector& objs) const; /// @brief perform collision test between one object and all the objects belonging to the manager void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; /// @brief perform distance computation between one object and all the objects belonging to the manager void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const; /// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) void collide(void* cdata, CollisionCallBack callback) const; /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) void distance(void* cdata, DistanceCallBack callback) const; /// @brief perform collision test with objects belonging to another manager void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const; /// @brief perform distance test with objects belonging to another manager void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const; /// @brief whether the manager is empty bool empty() const; /// @brief the number of objects managed by the manager inline size_t size() const { return endpoints[0].size() / 2; } protected: /// @brief SAP end point struct EndPoint { /// @brief object related with the end point CollisionObject* obj; /// @brief end point value FCL_REAL value; /// @brief tag for whether it is a lower bound or higher bound of an interval, 0 for lo, and 1 for hi char minmax; bool operator<(const EndPoint &p) const { return value < p.value; } }; /// @brief Extention interval tree's interval to SAP interval, adding more information struct SAPInterval : public SimpleInterval { CollisionObject* obj; SAPInterval(double low_, double high_, CollisionObject* obj_) : SimpleInterval() { low = low_; high = high_; obj = obj_; } }; bool checkColl(std::deque::const_iterator pos_start, std::deque::const_iterator pos_end, CollisionObject* obj, void* cdata, CollisionCallBack callback) const; bool checkDist(std::deque::const_iterator pos_start, std::deque::const_iterator pos_end, CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const; bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const; /// @brief vector stores all the end points std::vector endpoints[3]; /// @brief interval tree manages the intervals IntervalTree* interval_trees[3]; std::map obj_interval_maps[3]; /// @brief tag for whether the interval tree is maintained suitably bool setup_; }; } #endif fcl-0.5.0/include/fcl/broadphase/broadphase_spatialhash.h000066400000000000000000000164441274356571000234440ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_BROAD_PHASE_SPATIAL_HASH_H #define FCL_BROAD_PHASE_SPATIAL_HASH_H #include "fcl/broadphase/broadphase.h" #include "fcl/broadphase/hash.h" #include "fcl/BV/AABB.h" #include #include namespace fcl { /// @brief Spatial hash function: hash an AABB to a set of integer values struct SpatialHash { SpatialHash(const AABB& scene_limit_, FCL_REAL cell_size_) : cell_size(cell_size_), scene_limit(scene_limit_) { width[0] = std::ceil(scene_limit.width() / cell_size); width[1] = std::ceil(scene_limit.height() / cell_size); width[2] = std::ceil(scene_limit.depth() / cell_size); } std::vector operator() (const AABB& aabb) const { int min_x = std::floor((aabb.min_[0] - scene_limit.min_[0]) / cell_size); int max_x = std::ceil((aabb.max_[0] - scene_limit.min_[0]) / cell_size); int min_y = std::floor((aabb.min_[1] - scene_limit.min_[1]) / cell_size); int max_y = std::ceil((aabb.max_[1] - scene_limit.min_[1]) / cell_size); int min_z = std::floor((aabb.min_[2] - scene_limit.min_[2]) / cell_size); int max_z = std::ceil((aabb.max_[2] - scene_limit.min_[2]) / cell_size); std::vector keys((max_x - min_x) * (max_y - min_y) * (max_z - min_z)); int id = 0; for(int x = min_x; x < max_x; ++x) { for(int y = min_y; y < max_y; ++y) { for(int z = min_z; z < max_z; ++z) { keys[id++] = x + y * width[0] + z * width[0] * width[1]; } } } return keys; } private: FCL_REAL cell_size; AABB scene_limit; unsigned int width[3]; }; /// @brief spatial hashing collision mananger template > class SpatialHashingCollisionManager : public BroadPhaseCollisionManager { public: SpatialHashingCollisionManager(FCL_REAL cell_size, const Vec3f& scene_min, const Vec3f& scene_max, unsigned int default_table_size = 1000) : scene_limit(AABB(scene_min, scene_max)), hash_table(new HashTable(SpatialHash(scene_limit, cell_size))) { hash_table->init(default_table_size); } ~SpatialHashingCollisionManager() { delete hash_table; } /// @brief add one object to the manager void registerObject(CollisionObject* obj); /// @brief remove one object from the manager void unregisterObject(CollisionObject* obj); /// @brief initialize the manager, related with the specific type of manager void setup(); /// @brief update the condition of manager void update(); /// @brief update the manager by explicitly given the object updated void update(CollisionObject* updated_obj); /// @brief update the manager by explicitly given the set of objects update void update(const std::vector& updated_objs); /// @brief clear the manager void clear(); /// @brief return the objects managed by the manager void getObjects(std::vector& objs) const; /// @brief perform collision test between one object and all the objects belonging to the manager void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; /// @brief perform distance computation between one object and all the objects belonging ot the manager void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const; /// @brief perform collision test for the objects belonging to the manager (i.e, N^2 self collision) void collide(void* cdata, CollisionCallBack callback) const; /// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) void distance(void* cdata, DistanceCallBack callback) const; /// @brief perform collision test with objects belonging to another manager void collide(BroadPhaseCollisionManager* other_manager, void* cdata, CollisionCallBack callback) const; /// @brief perform distance test with objects belonging to another manager void distance(BroadPhaseCollisionManager* other_manager, void* cdata, DistanceCallBack callback) const; /// @brief whether the manager is empty bool empty() const; /// @brief the number of objects managed by the manager size_t size() const; /// @brief compute the bound for the environent static void computeBound(std::vector& objs, Vec3f& l, Vec3f& u) { AABB bound; for(unsigned int i = 0; i < objs.size(); ++i) bound += objs[i]->getAABB(); l = bound.min_; u = bound.max_; } protected: /// @brief perform collision test between one object and all the objects belonging to the manager bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; /// @brief perform distance computation between one object and all the objects belonging ot the manager bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const; /// @brief all objects in the scene std::list objs; /// @brief objects outside the scene limit are in another list std::list objs_outside_scene_limit; /// @brief the size of the scene AABB scene_limit; /// @brief store the map between objects and their aabbs. will make update more convenient std::map obj_aabb_map; /// @brief objects in the scene limit (given by scene_min and scene_max) are in the spatial hash table HashTable* hash_table; }; } #include "fcl/broadphase/broadphase_spatialhash.hxx" #endif fcl-0.5.0/include/fcl/broadphase/broadphase_spatialhash.hxx000066400000000000000000000333751274356571000240260ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ namespace fcl { template void SpatialHashingCollisionManager::registerObject(CollisionObject* obj) { objs.push_back(obj); const AABB& obj_aabb = obj->getAABB(); AABB overlap_aabb; if(scene_limit.overlap(obj_aabb, overlap_aabb)) { if(!scene_limit.contain(obj_aabb)) objs_outside_scene_limit.push_back(obj); hash_table->insert(overlap_aabb, obj); } else objs_outside_scene_limit.push_back(obj); obj_aabb_map[obj] = obj_aabb; } template void SpatialHashingCollisionManager::unregisterObject(CollisionObject* obj) { objs.remove(obj); const AABB& obj_aabb = obj->getAABB(); AABB overlap_aabb; if(scene_limit.overlap(obj_aabb, overlap_aabb)) { if(!scene_limit.contain(obj_aabb)) objs_outside_scene_limit.remove(obj); hash_table->remove(overlap_aabb, obj); } else objs_outside_scene_limit.remove(obj); obj_aabb_map.erase(obj); } template void SpatialHashingCollisionManager::setup() {} template void SpatialHashingCollisionManager::update() { hash_table->clear(); objs_outside_scene_limit.clear(); for(std::list::const_iterator it = objs.begin(), end = objs.end(); it != end; ++it) { CollisionObject* obj = *it; const AABB& obj_aabb = obj->getAABB(); AABB overlap_aabb; if(scene_limit.overlap(obj_aabb, overlap_aabb)) { if(!scene_limit.contain(obj_aabb)) objs_outside_scene_limit.push_back(obj); hash_table->insert(overlap_aabb, obj); } else objs_outside_scene_limit.push_back(obj); obj_aabb_map[obj] = obj_aabb; } } template void SpatialHashingCollisionManager::update(CollisionObject* updated_obj) { const AABB& new_aabb = updated_obj->getAABB(); const AABB& old_aabb = obj_aabb_map[updated_obj]; if(!scene_limit.contain(old_aabb)) // previously not completely in scene limit { if(scene_limit.contain(new_aabb)) { std::list::iterator find_it = std::find(objs_outside_scene_limit.begin(), objs_outside_scene_limit.end(), updated_obj); objs_outside_scene_limit.erase(find_it); } } else if(!scene_limit.contain(new_aabb)) // previous completely in scenelimit, now not objs_outside_scene_limit.push_back(updated_obj); AABB old_overlap_aabb; if(scene_limit.overlap(old_aabb, old_overlap_aabb)) hash_table->remove(old_overlap_aabb, updated_obj); AABB new_overlap_aabb; if(scene_limit.overlap(new_aabb, new_overlap_aabb)) hash_table->insert(new_overlap_aabb, updated_obj); obj_aabb_map[updated_obj] = new_aabb; } template void SpatialHashingCollisionManager::update(const std::vector& updated_objs) { for(size_t i = 0; i < updated_objs.size(); ++i) update(updated_objs[i]); } template void SpatialHashingCollisionManager::clear() { objs.clear(); hash_table->clear(); objs_outside_scene_limit.clear(); obj_aabb_map.clear(); } template void SpatialHashingCollisionManager::getObjects(std::vector& objs_) const { objs_.resize(objs.size()); std::copy(objs.begin(), objs.end(), objs_.begin()); } template void SpatialHashingCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const { if(size() == 0) return; collide_(obj, cdata, callback); } template void SpatialHashingCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const { if(size() == 0) return; FCL_REAL min_dist = std::numeric_limits::max(); distance_(obj, cdata, callback, min_dist); } template bool SpatialHashingCollisionManager::collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const { const AABB& obj_aabb = obj->getAABB(); AABB overlap_aabb; if(scene_limit.overlap(obj_aabb, overlap_aabb)) { if(!scene_limit.contain(obj_aabb)) { for(std::list::const_iterator it = objs_outside_scene_limit.begin(), end = objs_outside_scene_limit.end(); it != end; ++it) { if(obj == *it) continue; if(callback(obj, *it, cdata)) return true; } } std::vector query_result = hash_table->query(overlap_aabb); for(unsigned int i = 0; i < query_result.size(); ++i) { if(obj == query_result[i]) continue; if(callback(obj, query_result[i], cdata)) return true; } } else { ; for(std::list::const_iterator it = objs_outside_scene_limit.begin(), end = objs_outside_scene_limit.end(); it != end; ++it) { if(obj == *it) continue; if(callback(obj, *it, cdata)) return true; } } return false; } template bool SpatialHashingCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const { Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; AABB aabb = obj->getAABB(); if(min_dist < std::numeric_limits::max()) { Vec3f min_dist_delta(min_dist, min_dist, min_dist); aabb.expand(min_dist_delta); } AABB overlap_aabb; int status = 1; FCL_REAL old_min_distance; while(1) { old_min_distance = min_dist; if(scene_limit.overlap(aabb, overlap_aabb)) { if(!scene_limit.contain(aabb)) { for(std::list::const_iterator it = objs_outside_scene_limit.begin(), end = objs_outside_scene_limit.end(); it != end; ++it) { if(obj == *it) continue; if(!enable_tested_set_) { if(obj->getAABB().distance((*it)->getAABB()) < min_dist) if(callback(obj, *it, cdata, min_dist)) return true; } else { if(!inTestedSet(obj, *it)) { if(obj->getAABB().distance((*it)->getAABB()) < min_dist) if(callback(obj, *it, cdata, min_dist)) return true; insertTestedSet(obj, *it); } } } } std::vector query_result = hash_table->query(overlap_aabb); for(unsigned int i = 0; i < query_result.size(); ++i) { if(obj == query_result[i]) continue; if(!enable_tested_set_) { if(obj->getAABB().distance(query_result[i]->getAABB()) < min_dist) if(callback(obj, query_result[i], cdata, min_dist)) return true; } else { if(!inTestedSet(obj, query_result[i])) { if(obj->getAABB().distance(query_result[i]->getAABB()) < min_dist) if(callback(obj, query_result[i], cdata, min_dist)) return true; insertTestedSet(obj, query_result[i]); } } } } else { for(std::list::const_iterator it = objs_outside_scene_limit.begin(), end = objs_outside_scene_limit.end(); it != end; ++it) { if(obj == *it) continue; if(!enable_tested_set_) { if(obj->getAABB().distance((*it)->getAABB()) < min_dist) if(callback(obj, *it, cdata, min_dist)) return true; } else { if(!inTestedSet(obj, *it)) { if(obj->getAABB().distance((*it)->getAABB()) < min_dist) if(callback(obj, *it, cdata, min_dist)) return true; insertTestedSet(obj, *it); } } } } if(status == 1) { if(old_min_distance < std::numeric_limits::max()) break; else { if(min_dist < old_min_distance) { Vec3f min_dist_delta(min_dist, min_dist, min_dist); aabb = AABB(obj->getAABB(), min_dist_delta); status = 0; } else { if(aabb.equal(obj->getAABB())) aabb.expand(delta); else aabb.expand(obj->getAABB(), 2.0); } } } else if(status == 0) break; } return false; } template void SpatialHashingCollisionManager::collide(void* cdata, CollisionCallBack callback) const { if(size() == 0) return; for(std::list::const_iterator it1 = objs.begin(), end1 = objs.end(); it1 != end1; ++it1) { const AABB& obj_aabb = (*it1)->getAABB(); AABB overlap_aabb; if(scene_limit.overlap(obj_aabb, overlap_aabb)) { if(!scene_limit.contain(obj_aabb)) { for(std::list::const_iterator it2 = objs_outside_scene_limit.begin(), end2 = objs_outside_scene_limit.end(); it2 != end2; ++it2) { if(*it1 < *it2) { if(callback(*it1, *it2, cdata)) return; } } } std::vector query_result = hash_table->query(overlap_aabb); for(unsigned int i = 0; i < query_result.size(); ++i) { if(*it1 < query_result[i]) { if(callback(*it1, query_result[i], cdata)) return; } } } else { for(std::list::const_iterator it2 = objs_outside_scene_limit.begin(), end2 = objs_outside_scene_limit.end(); it2 != end2; ++it2) { if(*it1 < *it2) { if(callback(*it1, *it2, cdata)) return; } } } } } template void SpatialHashingCollisionManager::distance(void* cdata, DistanceCallBack callback) const { if(size() == 0) return; enable_tested_set_ = true; tested_set.clear(); FCL_REAL min_dist = std::numeric_limits::max(); for(std::list::const_iterator it = objs.begin(), end = objs.end(); it != end; ++it) if(distance_(*it, cdata, callback, min_dist)) break; enable_tested_set_ = false; tested_set.clear(); } template void SpatialHashingCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const { SpatialHashingCollisionManager* other_manager = static_cast* >(other_manager_); if((size() == 0) || (other_manager->size() == 0)) return; if(this == other_manager) { collide(cdata, callback); return; } if(this->size() < other_manager->size()) { for(std::list::const_iterator it = objs.begin(), end = objs.end(); it != end; ++it) if(other_manager->collide_(*it, cdata, callback)) return; } else { for(std::list::const_iterator it = other_manager->objs.begin(), end = other_manager->objs.end(); it != end; ++it) if(collide_(*it, cdata, callback)) return; } } template void SpatialHashingCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const { SpatialHashingCollisionManager* other_manager = static_cast* >(other_manager_); if((size() == 0) || (other_manager->size() == 0)) return; if(this == other_manager) { distance(cdata, callback); return; } FCL_REAL min_dist = std::numeric_limits::max(); if(this->size() < other_manager->size()) { for(std::list::const_iterator it = objs.begin(), end = objs.end(); it != end; ++it) if(other_manager->distance_(*it, cdata, callback, min_dist)) return; } else { for(std::list::const_iterator it = other_manager->objs.begin(), end = other_manager->objs.end(); it != end; ++it) if(distance_(*it, cdata, callback, min_dist)) return; } } template bool SpatialHashingCollisionManager::empty() const { return objs.empty(); } template size_t SpatialHashingCollisionManager::size() const { return objs.size(); } } fcl-0.5.0/include/fcl/broadphase/hash.h000066400000000000000000000126361274356571000176750ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_HASH_H #define FCL_HASH_H #include #include #include #include #include namespace fcl { /// @brief A simple hash table implemented as multiple buckets. HashFnc is any extended hash function: HashFnc(key) = {index1, index2, ..., } template class SimpleHashTable { protected: typedef std::list Bin; std::vector table_; HashFnc h_; size_t table_size_; public: SimpleHashTable(const HashFnc& h) : h_(h) { } ///@ brief Init the number of bins in the hash table void init(size_t size) { if(size == 0) { throw std::logic_error("SimpleHashTable must have non-zero size."); } table_.resize(size); table_size_ = size; } //// @brief Insert a key-value pair into the table void insert(Key key, Data value) { std::vector indices = h_(key); size_t range = table_.size(); for(size_t i = 0; i < indices.size(); ++i) table_[indices[i] % range].push_back(value); } /// @brief Find the elements in the hash table whose key is the same as query key. std::vector query(Key key) const { size_t range = table_.size(); std::vector indices = h_(key); std::set result; for(size_t i = 0; i < indices.size(); ++i) { unsigned int index = indices[i] % range; std::copy(table_[index].begin(), table_[index].end(), std::inserter(result, result.end())); } return std::vector(result.begin(), result.end()); } /// @brief remove the key-value pair from the table void remove(Key key, Data value) { size_t range = table_.size(); std::vector indices = h_(key); for(size_t i = 0; i < indices.size(); ++i) { unsigned int index = indices[i] % range; table_[index].remove(value); } } /// @brief clear the hash table void clear() { table_.clear(); table_.resize(table_size_); } }; template class unordered_map_hash_table : public std::unordered_map {}; /// @brief A hash table implemented using unordered_map template class TableT = unordered_map_hash_table> class SparseHashTable { protected: HashFnc h_; typedef std::list Bin; typedef TableT Table; Table table_; public: SparseHashTable(const HashFnc& h) : h_(h) {} /// @brief Init the hash table. The bucket size is dynamically decided. void init(size_t) { table_.clear(); } /// @brief insert one key-value pair into the hash table void insert(Key key, Data value) { std::vector indices = h_(key); for(size_t i = 0; i < indices.size(); ++i) table_[indices[i]].push_back(value); } /// @brief find the elements whose key is the same as the query std::vector query(Key key) const { std::vector indices = h_(key); std::set result; for(size_t i = 0; i < indices.size(); ++i) { unsigned int index = indices[i]; typename Table::const_iterator p = table_.find(index); if(p != table_.end()) std::copy((*p).second.begin(), (*p).second.end(), std::inserter(result, result.end())); } return std::vector(result.begin(), result.end()); } /// @brief remove one key-value pair from the hash table void remove(Key key, Data value) { std::vector indices = h_(key); for(size_t i = 0; i < indices.size(); ++i) { unsigned int index = indices[i]; table_[index].remove(value); } } /// @brief clear the hash table void clear() { table_.clear(); } }; } #endif fcl-0.5.0/include/fcl/broadphase/hierarchy_tree.h000066400000000000000000000461511274356571000217460ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_HIERARCHY_TREE_H #define FCL_HIERARCHY_TREE_H #include #include #include #include "fcl/BV/AABB.h" #include "fcl/broadphase/morton.h" namespace fcl { /// @brief dynamic AABB tree node template struct NodeBase { /// @brief the bounding volume for the node BV bv; /// @brief pointer to parent node NodeBase* parent; /// @brief whether is a leaf bool isLeaf() const { return (children[1] == NULL); } /// @brief whether is internal node bool isInternal() const { return !isLeaf(); } union { /// @brief for leaf node, children nodes NodeBase* children[2]; void* data; }; /// @brief morton code for current BV FCL_UINT32 code; NodeBase() { parent = NULL; children[0] = NULL; children[1] = NULL; } }; /// @brief Compare two nodes accoording to the d-th dimension of node center template bool nodeBaseLess(NodeBase* a, NodeBase* b, int d) { if(a->bv.center()[d] < b->bv.center()[d]) return true; return false; } /// @brief select from node1 and node2 which is close to a given query. 0 for node1 and 1 for node2 template size_t select(const NodeBase& query, const NodeBase& node1, const NodeBase& node2) { return 0; } template<> size_t select(const NodeBase& node, const NodeBase& node1, const NodeBase& node2); /// @brief select from node1 and node2 which is close to a given query bounding volume. 0 for node1 and 1 for node2 template size_t select(const BV& query, const NodeBase& node1, const NodeBase& node2) { return 0; } template<> size_t select(const AABB& query, const NodeBase& node1, const NodeBase& node2); /// @brief Class for hierarchy tree structure template class HierarchyTree { typedef NodeBase NodeType; typedef typename std::vector* >::iterator NodeVecIterator; typedef typename std::vector* >::const_iterator NodeVecConstIterator; struct SortByMorton { bool operator() (const NodeType* a, const NodeType* b) const { return a->code < b->code; } }; public: /// @brief Create hierarchy tree with suitable setting. /// bu_threshold decides the height of tree node to start bottom-up construction / optimization; /// topdown_level decides different methods to construct tree in topdown manner. /// lower level method constructs tree with better quality but is slower. HierarchyTree(int bu_threshold_ = 16, int topdown_level_ = 0); ~HierarchyTree(); /// @brief Initialize the tree by a set of leaves using algorithm with a given level. void init(std::vector& leaves, int level = 0); /// @brief Insest a node NodeType* insert(const BV& bv, void* data); /// @brief Remove a leaf node void remove(NodeType* leaf); /// @brief Clear the tree void clear(); /// @brief Whether the tree is empty bool empty() const; /// @brief update one leaf node void update(NodeType* leaf, int lookahead_level = -1); /// @brief update the tree when the bounding volume of a given leaf has changed bool update(NodeType* leaf, const BV& bv); /// @brief update one leaf's bounding volume, with prediction bool update(NodeType* leaf, const BV& bv, const Vec3f& vel, FCL_REAL margin); /// @brief update one leaf's bounding volume, with prediction bool update(NodeType* leaf, const BV& bv, const Vec3f& vel); /// @brief get the max height of the tree size_t getMaxHeight() const; /// @brief get the max depth of the tree size_t getMaxDepth() const; /// @brief balance the tree from bottom void balanceBottomup(); /// @brief balance the tree from top void balanceTopdown(); /// @brief balance the tree in an incremental way void balanceIncremental(int iterations); /// @brief refit the tree, i.e., when the leaf nodes' bounding volumes change, update the entire tree in a bottom-up manner void refit(); /// @brief extract all the leaves of the tree void extractLeaves(const NodeType* root, std::vector& leaves) const; /// @brief number of leaves in the tree size_t size() const; /// @brief get the root of the tree NodeType* getRoot() const; NodeType*& getRoot(); /// @brief print the tree in a recursive way void print(NodeType* root, int depth); private: /// @brief construct a tree for a set of leaves from bottom -- very heavy way void bottomup(const NodeVecIterator lbeg, const NodeVecIterator lend); /// @brief construct a tree for a set of leaves from top NodeType* topdown(const NodeVecIterator lbeg, const NodeVecIterator lend); /// @brief compute the maximum height of a subtree rooted from a given node size_t getMaxHeight(NodeType* node) const; /// @brief compute the maximum depth of a subtree rooted from a given node void getMaxDepth(NodeType* node, size_t depth, size_t& max_depth) const; /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a topdown manner. /// During construction, first compute the best split axis as the axis along with the longest AABB edge. /// Then compute the median of all nodes' center projection onto the axis and using it as the split threshold. NodeType* topdown_0(const NodeVecIterator lbeg, const NodeVecIterator lend); /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a topdown manner. /// During construction, first compute the best split thresholds for different axes as the average of all nodes' center. /// Then choose the split axis as the axis whose threshold can divide the nodes into two parts with almost similar size. /// This construction is more expensive then topdown_0, but also can provide tree with better quality. NodeType* topdown_1(const NodeVecIterator lbeg, const NodeVecIterator lend); /// @brief init tree from leaves in the topdown manner (topdown_0 or topdown_1) void init_0(std::vector& leaves); /// @brief init tree from leaves using morton code. It uses morton_0, i.e., for nodes which is of depth more than the maximum bits of the morton code, /// we use bottomup method to construct the subtree, which is slow but can construct tree with high quality. void init_1(std::vector& leaves); /// @brief init tree from leaves using morton code. It uses morton_0, i.e., for nodes which is of depth more than the maximum bits of the morton code, /// we split the leaves into two parts with the same size simply using the node index. void init_2(std::vector& leaves); /// @brief init tree from leaves using morton code. It uses morton_2, i.e., for all nodes, we simply divide the leaves into parts with the same size simply using the node index. void init_3(std::vector& leaves); NodeType* mortonRecurse_0(const NodeVecIterator lbeg, const NodeVecIterator lend, const FCL_UINT32& split, int bits); NodeType* mortonRecurse_1(const NodeVecIterator lbeg, const NodeVecIterator lend, const FCL_UINT32& split, int bits); NodeType* mortonRecurse_2(const NodeVecIterator lbeg, const NodeVecIterator lend); /// @brief update one leaf node's bounding volume void update_(NodeType* leaf, const BV& bv); /// @brief sort node n and its parent according to their memory position NodeType* sort(NodeType* n, NodeType*& r); /// @brief Insert a leaf node and also update its ancestors void insertLeaf(NodeType* root, NodeType* leaf); /// @brief Remove a leaf. The leaf node itself is not deleted yet, but all the unnecessary internal nodes are deleted. /// return the node with the smallest depth and is influenced by the remove operation NodeType* removeLeaf(NodeType* leaf); /// @brief Delete all internal nodes and return all leaves nodes with given depth from root void fetchLeaves(NodeType* root, std::vector& leaves, int depth = -1); static size_t indexOf(NodeType* node); /// @brief create one node (leaf or internal) NodeType* createNode(NodeType* parent, const BV& bv, void* data); NodeType* createNode(NodeType* parent, const BV& bv1, const BV& bv2, void* data); NodeType* createNode(NodeType* parent, void* data); void deleteNode(NodeType* node); void recurseDeleteNode(NodeType* node); void recurseRefit(NodeType* node); static BV bounds(const std::vector& leaves); static BV bounds(const NodeVecIterator lbeg, const NodeVecIterator lend); protected: NodeType* root_node; size_t n_leaves; unsigned int opath; /// This is a one NodeType cache, the reason is that we need to remove a node and then add it again frequently. NodeType* free_node; int max_lookahead_level; public: /// @brief decide which topdown algorithm to use int topdown_level; /// @brief decide the depth to use expensive bottom-up algorithm int bu_threshold; }; template<> bool HierarchyTree::update(NodeBase* leaf, const AABB& bv_, const Vec3f& vel, FCL_REAL margin); template<> bool HierarchyTree::update(NodeBase* leaf, const AABB& bv_, const Vec3f& vel); namespace implementation_array { template struct NodeBase { BV bv; union { size_t parent; size_t next; }; union { size_t children[2]; void* data; }; FCL_UINT32 code; bool isLeaf() const { return (children[1] == (size_t)(-1)); } bool isInternal() const { return !isLeaf(); } }; /// @brief Functor comparing two nodes template struct nodeBaseLess { nodeBaseLess(const NodeBase* nodes_, size_t d_) : nodes(nodes_), d(d_) {} bool operator() (size_t i, size_t j) const { if(nodes[i].bv.center()[d] < nodes[j].bv.center()[d]) return true; return false; } private: /// @brief the nodes array const NodeBase* nodes; /// @brief the dimension to compare size_t d; }; /// @brief select the node from node1 and node2 which is close to the query-th node in the nodes. 0 for node1 and 1 for node2. template size_t select(size_t query, size_t node1, size_t node2, NodeBase* nodes) { return 0; } template<> size_t select(size_t query, size_t node1, size_t node2, NodeBase* nodes); /// @brief select the node from node1 and node2 which is close to the query AABB. 0 for node1 and 1 for node2. template size_t select(const BV& query, size_t node1, size_t node2, NodeBase* nodes) { return 0; } template<> size_t select(const AABB& query, size_t node1, size_t node2, NodeBase* nodes); /// @brief Class for hierarchy tree structure template class HierarchyTree { typedef NodeBase NodeType; struct SortByMorton { bool operator() (size_t a, size_t b) const { if((a != NULL_NODE) && (b != NULL_NODE)) return nodes[a].code < nodes[b].code; else if(a == NULL_NODE) return split < nodes[b].code; else if(b == NULL_NODE) return nodes[a].code < split; return false; } NodeType* nodes; FCL_UINT32 split; }; public: /// @brief Create hierarchy tree with suitable setting. /// bu_threshold decides the height of tree node to start bottom-up construction / optimization; /// topdown_level decides different methods to construct tree in topdown manner. /// lower level method constructs tree with better quality but is slower. HierarchyTree(int bu_threshold_ = 16, int topdown_level_ = 0); ~HierarchyTree(); /// @brief Initialize the tree by a set of leaves using algorithm with a given level. void init(NodeType* leaves, int n_leaves_, int level = 0); /// @brief Initialize the tree by a set of leaves using algorithm with a given level. size_t insert(const BV& bv, void* data); /// @brief Remove a leaf node void remove(size_t leaf); /// @brief Clear the tree void clear(); /// @brief Whether the tree is empty bool empty() const; /// @brief update one leaf node void update(size_t leaf, int lookahead_level = -1); /// @brief update the tree when the bounding volume of a given leaf has changed bool update(size_t leaf, const BV& bv); /// @brief update one leaf's bounding volume, with prediction bool update(size_t leaf, const BV& bv, const Vec3f& vel, FCL_REAL margin); /// @brief update one leaf's bounding volume, with prediction bool update(size_t leaf, const BV& bv, const Vec3f& vel); /// @brief get the max height of the tree size_t getMaxHeight() const; /// @brief get the max depth of the tree size_t getMaxDepth() const; /// @brief balance the tree from bottom void balanceBottomup(); /// @brief balance the tree from top void balanceTopdown(); /// @brief balance the tree in an incremental way void balanceIncremental(int iterations); /// @brief refit the tree, i.e., when the leaf nodes' bounding volumes change, update the entire tree in a bottom-up manner void refit(); /// @brief extract all the leaves of the tree void extractLeaves(size_t root, NodeType*& leaves) const; /// @brief number of leaves in the tree size_t size() const; /// @brief get the root of the tree size_t getRoot() const; /// @brief get the pointer to the nodes array NodeType* getNodes() const; /// @brief print the tree in a recursive way void print(size_t root, int depth); private: /// @brief construct a tree for a set of leaves from bottom -- very heavy way void bottomup(size_t* lbeg, size_t* lend); /// @brief construct a tree for a set of leaves from top size_t topdown(size_t* lbeg, size_t* lend); /// @brief compute the maximum height of a subtree rooted from a given node size_t getMaxHeight(size_t node) const; /// @brief compute the maximum depth of a subtree rooted from a given node void getMaxDepth(size_t node, size_t depth, size_t& max_depth) const; /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a topdown manner. /// During construction, first compute the best split axis as the axis along with the longest AABB edge. /// Then compute the median of all nodes' center projection onto the axis and using it as the split threshold. size_t topdown_0(size_t* lbeg, size_t* lend); /// @brief construct a tree from a list of nodes stored in [lbeg, lend) in a topdown manner. /// During construction, first compute the best split thresholds for different axes as the average of all nodes' center. /// Then choose the split axis as the axis whose threshold can divide the nodes into two parts with almost similar size. /// This construction is more expensive then topdown_0, but also can provide tree with better quality. size_t topdown_1(size_t* lbeg, size_t* lend); /// @brief init tree from leaves in the topdown manner (topdown_0 or topdown_1) void init_0(NodeType* leaves, int n_leaves_); /// @brief init tree from leaves using morton code. It uses morton_0, i.e., for nodes which is of depth more than the maximum bits of the morton code, /// we use bottomup method to construct the subtree, which is slow but can construct tree with high quality. void init_1(NodeType* leaves, int n_leaves_); /// @brief init tree from leaves using morton code. It uses morton_0, i.e., for nodes which is of depth more than the maximum bits of the morton code, /// we split the leaves into two parts with the same size simply using the node index. void init_2(NodeType* leaves, int n_leaves_); /// @brief init tree from leaves using morton code. It uses morton_2, i.e., for all nodes, we simply divide the leaves into parts with the same size simply using the node index. void init_3(NodeType* leaves, int n_leaves_); size_t mortonRecurse_0(size_t* lbeg, size_t* lend, const FCL_UINT32& split, int bits); size_t mortonRecurse_1(size_t* lbeg, size_t* lend, const FCL_UINT32& split, int bits); size_t mortonRecurse_2(size_t* lbeg, size_t* lend); /// @brief update one leaf node's bounding volume void update_(size_t leaf, const BV& bv); /// @brief Insert a leaf node and also update its ancestors void insertLeaf(size_t root, size_t leaf); /// @brief Remove a leaf. The leaf node itself is not deleted yet, but all the unnecessary internal nodes are deleted. /// return the node with the smallest depth and is influenced by the remove operation size_t removeLeaf(size_t leaf); /// @brief Delete all internal nodes and return all leaves nodes with given depth from root void fetchLeaves(size_t root, NodeType*& leaves, int depth = -1); size_t indexOf(size_t node); size_t allocateNode(); /// @brief create one node (leaf or internal) size_t createNode(size_t parent, const BV& bv1, const BV& bv2, void* data); size_t createNode(size_t parent, const BV& bv, void* data); size_t createNode(size_t parent, void* data); void deleteNode(size_t node); void recurseRefit(size_t node); protected: size_t root_node; NodeType* nodes; size_t n_nodes; size_t n_nodes_alloc; size_t n_leaves; size_t freelist; unsigned int opath; int max_lookahead_level; public: /// @brief decide which topdown algorithm to use int topdown_level; /// @brief decide the depth to use expensive bottom-up algorithm int bu_threshold; public: static const size_t NULL_NODE = -1; }; template const size_t HierarchyTree::NULL_NODE; } } #include "fcl/broadphase/hierarchy_tree.hxx" #endif fcl-0.5.0/include/fcl/broadphase/hierarchy_tree.hxx000066400000000000000000001315721274356571000223300ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ namespace fcl { template HierarchyTree::HierarchyTree(int bu_threshold_, int topdown_level_) { root_node = NULL; n_leaves = 0; free_node = NULL; max_lookahead_level = -1; opath = 0; bu_threshold = bu_threshold_; topdown_level = topdown_level_; } template HierarchyTree::~HierarchyTree() { clear(); } template void HierarchyTree::init(std::vector& leaves, int level) { switch(level) { case 0: init_0(leaves); break; case 1: init_1(leaves); break; case 2: init_2(leaves); break; case 3: init_3(leaves); break; default: init_0(leaves); } } template typename HierarchyTree::NodeType* HierarchyTree::insert(const BV& bv, void* data) { NodeType* leaf = createNode(NULL, bv, data); insertLeaf(root_node, leaf); ++n_leaves; return leaf; } template void HierarchyTree::remove(NodeType* leaf) { removeLeaf(leaf); deleteNode(leaf); --n_leaves; } template void HierarchyTree::clear() { if(root_node) recurseDeleteNode(root_node); n_leaves = 0; delete free_node; free_node = NULL; max_lookahead_level = -1; opath = 0; } template bool HierarchyTree::empty() const { return (NULL == root_node); } template void HierarchyTree::update(NodeType* leaf, int lookahead_level) { NodeType* root = removeLeaf(leaf); if(root) { if(lookahead_level > 0) { for(int i = 0; (i < lookahead_level) && root->parent; ++i) root = root->parent; } else root = root_node; } insertLeaf(root, leaf); } template bool HierarchyTree::update(NodeType* leaf, const BV& bv) { if(leaf->bv.contain(bv)) return false; update_(leaf, bv); return true; } template bool HierarchyTree::update(NodeType* leaf, const BV& bv, const Vec3f& vel, FCL_REAL margin) { if(leaf->bv.contain(bv)) return false; update_(leaf, bv); return true; } template bool HierarchyTree::update(NodeType* leaf, const BV& bv, const Vec3f& vel) { if(leaf->bv.contain(bv)) return false; update_(leaf, bv); return true; } template size_t HierarchyTree::getMaxHeight() const { if(!root_node) return 0; return getMaxHeight(root_node); } template size_t HierarchyTree::getMaxDepth() const { if(!root_node) return 0; size_t max_depth; getMaxDepth(root_node, 0, max_depth); return max_depth; } template void HierarchyTree::balanceBottomup() { if(root_node) { std::vector leaves; leaves.reserve(n_leaves); fetchLeaves(root_node, leaves); bottomup(leaves.begin(), leaves.end()); root_node = leaves[0]; } } template void HierarchyTree::balanceTopdown() { if(root_node) { std::vector leaves; leaves.reserve(n_leaves); fetchLeaves(root_node, leaves); root_node = topdown(leaves.begin(), leaves.end()); } } template void HierarchyTree::balanceIncremental(int iterations) { if(iterations < 0) iterations = n_leaves; if(root_node && (iterations > 0)) { for(int i = 0; i < iterations; ++i) { NodeType* node = root_node; unsigned int bit = 0; while(!node->isLeaf()) { node = sort(node, root_node)->children[(opath>>bit)&1]; bit = (bit+1)&(sizeof(unsigned int) * 8-1); } update(node); ++opath; } } } template void HierarchyTree::refit() { if(root_node) recurseRefit(root_node); } template void HierarchyTree::extractLeaves(const NodeType* root, std::vector& leaves) const { if(!root->isLeaf()) { extractLeaves(root->children[0], leaves); extractLeaves(root->children[1], leaves); } else leaves.push_back(root); } template size_t HierarchyTree::size() const { return n_leaves; } template typename HierarchyTree::NodeType* HierarchyTree::getRoot() const { return root_node; } template typename HierarchyTree::NodeType*& HierarchyTree::getRoot() { return root_node; } template void HierarchyTree::print(NodeType* root, int depth) { for(int i = 0; i < depth; ++i) std::cout << " "; std::cout << " (" << root->bv.min_[0] << ", " << root->bv.min_[1] << ", " << root->bv.min_[2] << "; " << root->bv.max_[0] << ", " << root->bv.max_[1] << ", " << root->bv.max_[2] << ")" << std::endl; if(root->isLeaf()) { } else { print(root->children[0], depth+1); print(root->children[1], depth+1); } } template void HierarchyTree::bottomup(const NodeVecIterator lbeg, const NodeVecIterator lend) { NodeVecIterator lcur_end = lend; while(lbeg < lcur_end - 1) { NodeVecIterator min_it1, min_it2; FCL_REAL min_size = std::numeric_limits::max(); for(NodeVecIterator it1 = lbeg; it1 < lcur_end; ++it1) { for(NodeVecIterator it2 = it1 + 1; it2 < lcur_end; ++it2) { FCL_REAL cur_size = ((*it1)->bv + (*it2)->bv).size(); if(cur_size < min_size) { min_size = cur_size; min_it1 = it1; min_it2 = it2; } } } NodeType* n[2] = {*min_it1, *min_it2}; NodeType* p = createNode(NULL, n[0]->bv, n[1]->bv, NULL); p->children[0] = n[0]; p->children[1] = n[1]; n[0]->parent = p; n[1]->parent = p; *min_it1 = p; NodeType* tmp = *min_it2; lcur_end--; *min_it2 = *lcur_end; *lcur_end = tmp; } } template typename HierarchyTree::NodeType* HierarchyTree::topdown(const NodeVecIterator lbeg, const NodeVecIterator lend) { switch(topdown_level) { case 0: return topdown_0(lbeg, lend); break; case 1: return topdown_1(lbeg, lend); break; default: return topdown_0(lbeg, lend); } } template size_t HierarchyTree::getMaxHeight(NodeType* node) const { if(!node->isLeaf()) { size_t height1 = getMaxHeight(node->children[0]); size_t height2 = getMaxHeight(node->children[1]); return std::max(height1, height2) + 1; } else return 0; } template void HierarchyTree::getMaxDepth(NodeType* node, size_t depth, size_t& max_depth) const { if(!node->isLeaf()) { getMaxDepth(node->children[0], depth+1, max_depth); getMaxDepth(node->children[1], depth+1, max_depth); } else max_depth = std::max(max_depth, depth); } template typename HierarchyTree::NodeType* HierarchyTree::topdown_0(const NodeVecIterator lbeg, const NodeVecIterator lend) { int num_leaves = lend - lbeg; if(num_leaves > 1) { if(num_leaves > bu_threshold) { BV vol = (*lbeg)->bv; for(NodeVecIterator it = lbeg + 1; it < lend; ++it) vol += (*it)->bv; int best_axis = 0; FCL_REAL extent[3] = {vol.width(), vol.height(), vol.depth()}; if(extent[1] > extent[0]) best_axis = 1; if(extent[2] > extent[best_axis]) best_axis = 2; // compute median NodeVecIterator lcenter = lbeg + num_leaves / 2; std::nth_element(lbeg, lcenter, lend, std::bind(&nodeBaseLess, std::placeholders::_1, std::placeholders::_2, std::ref(best_axis))); NodeType* node = createNode(NULL, vol, NULL); node->children[0] = topdown_0(lbeg, lcenter); node->children[1] = topdown_0(lcenter, lend); node->children[0]->parent = node; node->children[1]->parent = node; return node; } else { bottomup(lbeg, lend); return *lbeg; } } return *lbeg; } template typename HierarchyTree::NodeType* HierarchyTree::topdown_1(const NodeVecIterator lbeg, const NodeVecIterator lend) { int num_leaves = lend - lbeg; if(num_leaves > 1) { if(num_leaves > bu_threshold) { Vec3f split_p = (*lbeg)->bv.center(); BV vol = (*lbeg)->bv; NodeVecIterator it; for(it = lbeg + 1; it < lend; ++it) { split_p += (*it)->bv.center(); vol += (*it)->bv; } split_p /= (FCL_REAL)(num_leaves); int best_axis = -1; int bestmidp = num_leaves; int splitcount[3][2] = {{0,0}, {0,0}, {0,0}}; for(it = lbeg; it < lend; ++it) { Vec3f x = (*it)->bv.center() - split_p; for(size_t j = 0; j < 3; ++j) ++splitcount[j][x[j] > 0 ? 1 : 0]; } for(size_t i = 0; i < 3; ++i) { if((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) { int midp = std::abs(splitcount[i][0] - splitcount[i][1]); if(midp < bestmidp) { best_axis = i; bestmidp = midp; } } } if(best_axis < 0) best_axis = 0; FCL_REAL split_value = split_p[best_axis]; NodeVecIterator lcenter = lbeg; for(it = lbeg; it < lend; ++it) { if((*it)->bv.center()[best_axis] < split_value) { NodeType* temp = *it; *it = *lcenter; *lcenter = temp; ++lcenter; } } NodeType* node = createNode(NULL, vol, NULL); node->children[0] = topdown_1(lbeg, lcenter); node->children[1] = topdown_1(lcenter, lend); node->children[0]->parent = node; node->children[1]->parent = node; return node; } else { bottomup(lbeg, lend); return *lbeg; } } return *lbeg; } template void HierarchyTree::init_0(std::vector& leaves) { clear(); root_node = topdown(leaves.begin(), leaves.end()); n_leaves = leaves.size(); max_lookahead_level = -1; opath = 0; } template void HierarchyTree::init_1(std::vector& leaves) { clear(); BV bound_bv; if(leaves.size() > 0) bound_bv = leaves[0]->bv; for(size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv; morton_functor coder(bound_bv); for(size_t i = 0; i < leaves.size(); ++i) leaves[i]->code = coder(leaves[i]->bv.center()); std::sort(leaves.begin(), leaves.end(), SortByMorton()); root_node = mortonRecurse_0(leaves.begin(), leaves.end(), (1 << (coder.bits()-1)), coder.bits()-1); refit(); n_leaves = leaves.size(); max_lookahead_level = -1; opath = 0; } template void HierarchyTree::init_2(std::vector& leaves) { clear(); BV bound_bv; if(leaves.size() > 0) bound_bv = leaves[0]->bv; for(size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv; morton_functor coder(bound_bv); for(size_t i = 0; i < leaves.size(); ++i) leaves[i]->code = coder(leaves[i]->bv.center()); std::sort(leaves.begin(), leaves.end(), SortByMorton()); root_node = mortonRecurse_1(leaves.begin(), leaves.end(), (1 << (coder.bits()-1)), coder.bits()-1); refit(); n_leaves = leaves.size(); max_lookahead_level = -1; opath = 0; } template void HierarchyTree::init_3(std::vector& leaves) { clear(); BV bound_bv; if(leaves.size() > 0) bound_bv = leaves[0]->bv; for(size_t i = 1; i < leaves.size(); ++i) bound_bv += leaves[i]->bv; morton_functor coder(bound_bv); for(size_t i = 0; i < leaves.size(); ++i) leaves[i]->code = coder(leaves[i]->bv.center()); std::sort(leaves.begin(), leaves.end(), SortByMorton()); root_node = mortonRecurse_2(leaves.begin(), leaves.end()); refit(); n_leaves = leaves.size(); max_lookahead_level = -1; opath = 0; } template typename HierarchyTree::NodeType* HierarchyTree::mortonRecurse_0(const NodeVecIterator lbeg, const NodeVecIterator lend, const FCL_UINT32& split, int bits) { int num_leaves = lend - lbeg; if(num_leaves > 1) { if(bits > 0) { NodeType dummy; dummy.code = split; NodeVecIterator lcenter = std::lower_bound(lbeg, lend, &dummy, SortByMorton()); if(lcenter == lbeg) { FCL_UINT32 split2 = split | (1 << (bits - 1)); return mortonRecurse_0(lbeg, lend, split2, bits - 1); } else if(lcenter == lend) { FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); return mortonRecurse_0(lbeg, lend, split1, bits - 1); } else { FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); FCL_UINT32 split2 = split | (1 << (bits - 1)); NodeType* child1 = mortonRecurse_0(lbeg, lcenter, split1, bits - 1); NodeType* child2 = mortonRecurse_0(lcenter, lend, split2, bits - 1); NodeType* node = createNode(NULL, NULL); node->children[0] = child1; node->children[1] = child2; child1->parent = node; child2->parent = node; return node; } } else { NodeType* node = topdown(lbeg, lend); return node; } } else return *lbeg; } template typename HierarchyTree::NodeType* HierarchyTree::mortonRecurse_1(const NodeVecIterator lbeg, const NodeVecIterator lend, const FCL_UINT32& split, int bits) { int num_leaves = lend - lbeg; if(num_leaves > 1) { if(bits > 0) { NodeType dummy; dummy.code = split; NodeVecIterator lcenter = std::lower_bound(lbeg, lend, &dummy, SortByMorton()); if(lcenter == lbeg) { FCL_UINT32 split2 = split | (1 << (bits - 1)); return mortonRecurse_1(lbeg, lend, split2, bits - 1); } else if(lcenter == lend) { FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); return mortonRecurse_1(lbeg, lend, split1, bits - 1); } else { FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); FCL_UINT32 split2 = split | (1 << (bits - 1)); NodeType* child1 = mortonRecurse_1(lbeg, lcenter, split1, bits - 1); NodeType* child2 = mortonRecurse_1(lcenter, lend, split2, bits - 1); NodeType* node = createNode(NULL, NULL); node->children[0] = child1; node->children[1] = child2; child1->parent = node; child2->parent = node; return node; } } else { NodeType* child1 = mortonRecurse_1(lbeg, lbeg + num_leaves / 2, 0, bits - 1); NodeType* child2 = mortonRecurse_1(lbeg + num_leaves / 2, lend, 0, bits - 1); NodeType* node = createNode(NULL, NULL); node->children[0] = child1; node->children[1] = child2; child1->parent = node; child2->parent = node; return node; } } else return *lbeg; } template typename HierarchyTree::NodeType* HierarchyTree::mortonRecurse_2(const NodeVecIterator lbeg, const NodeVecIterator lend) { int num_leaves = lend - lbeg; if(num_leaves > 1) { NodeType* child1 = mortonRecurse_2(lbeg, lbeg + num_leaves / 2); NodeType* child2 = mortonRecurse_2(lbeg + num_leaves / 2, lend); NodeType* node = createNode(NULL, NULL); node->children[0] = child1; node->children[1] = child2; child1->parent = node; child2->parent = node; return node; } else return *lbeg; } template void HierarchyTree::update_(NodeType* leaf, const BV& bv) { NodeType* root = removeLeaf(leaf); if(root) { if(max_lookahead_level >= 0) { for(int i = 0; (i < max_lookahead_level) && root->parent; ++i) root = root->parent; } else root = root_node; } leaf->bv = bv; insertLeaf(root, leaf); } template typename HierarchyTree::NodeType* HierarchyTree::sort(NodeType* n, NodeType*& r) { NodeType* p = n->parent; if(p > n) { int i = indexOf(n); int j = 1 - i; NodeType* s = p->children[j]; NodeType* q = p->parent; if(q) q->children[indexOf(p)] = n; else r = n; s->parent = n; p->parent = n; n->parent = q; p->children[0] = n->children[0]; p->children[1] = n->children[1]; n->children[0]->parent = p; n->children[1]->parent = p; n->children[i] = p; n->children[j] = s; std::swap(p->bv, n->bv); return p; } return n; } template void HierarchyTree::insertLeaf(NodeType* root, NodeType* leaf) { if(!root_node) { root_node = leaf; leaf->parent = NULL; } else { if(!root->isLeaf()) { do { root = root->children[select(*leaf, *(root->children[0]), *(root->children[1]))]; } while(!root->isLeaf()); } NodeType* prev = root->parent; NodeType* node = createNode(prev, leaf->bv, root->bv, NULL); if(prev) { prev->children[indexOf(root)] = node; node->children[0] = root; root->parent = node; node->children[1] = leaf; leaf->parent = node; do { if(!prev->bv.contain(node->bv)) prev->bv = prev->children[0]->bv + prev->children[1]->bv; else break; node = prev; } while (NULL != (prev = node->parent)); } else { node->children[0] = root; root->parent = node; node->children[1] = leaf; leaf->parent = node; root_node = node; } } } template typename HierarchyTree::NodeType* HierarchyTree::removeLeaf(NodeType* leaf) { if(leaf == root_node) { root_node = NULL; return NULL; } else { NodeType* parent = leaf->parent; NodeType* prev = parent->parent; NodeType* sibling = parent->children[1-indexOf(leaf)]; if(prev) { prev->children[indexOf(parent)] = sibling; sibling->parent = prev; deleteNode(parent); while(prev) { BV new_bv = prev->children[0]->bv + prev->children[1]->bv; if(!new_bv.equal(prev->bv)) { prev->bv = new_bv; prev = prev->parent; } else break; } return prev ? prev : root_node; } else { root_node = sibling; sibling->parent = NULL; deleteNode(parent); return root_node; } } } template void HierarchyTree::fetchLeaves(NodeType* root, std::vector& leaves, int depth) { if((!root->isLeaf()) && depth) { fetchLeaves(root->children[0], leaves, depth-1); fetchLeaves(root->children[1], leaves, depth-1); deleteNode(root); } else { leaves.push_back(root); } } template size_t HierarchyTree::indexOf(NodeType* node) { // node cannot be NULL return (node->parent->children[1] == node); } template typename HierarchyTree::NodeType* HierarchyTree::createNode(NodeType* parent, const BV& bv, void* data) { NodeType* node = createNode(parent, data); node->bv = bv; return node; } template typename HierarchyTree::NodeType* HierarchyTree::createNode(NodeType* parent, const BV& bv1, const BV& bv2, void* data) { NodeType* node = createNode(parent, data); node->bv = bv1 + bv2; return node; } template typename HierarchyTree::NodeType* HierarchyTree::createNode(NodeType* parent, void* data) { NodeType* node = NULL; if(free_node) { node = free_node; free_node = NULL; } else node = new NodeType(); node->parent = parent; node->data = data; node->children[1] = 0; return node; } template void HierarchyTree::deleteNode(NodeType* node) { if(free_node != node) { delete free_node; free_node = node; } } template void HierarchyTree::recurseDeleteNode(NodeType* node) { if(!node->isLeaf()) { recurseDeleteNode(node->children[0]); recurseDeleteNode(node->children[1]); } if(node == root_node) root_node = NULL; deleteNode(node); } template void HierarchyTree::recurseRefit(NodeType* node) { if(!node->isLeaf()) { recurseRefit(node->children[0]); recurseRefit(node->children[1]); node->bv = node->children[0]->bv + node->children[1]->bv; } else return; } template BV HierarchyTree::bounds(const std::vector& leaves) { if(leaves.size() == 0) return BV(); BV bv = leaves[0]->bv; for(size_t i = 1; i < leaves.size(); ++i) { bv += leaves[i]->bv; } return bv; } template BV HierarchyTree::bounds(const NodeVecIterator lbeg, const NodeVecIterator lend) { if(lbeg == lend) return BV(); BV bv = *lbeg; for(NodeVecIterator it = lbeg + 1; it < lend; ++it) { bv += (*it)->bv; } return bv; } } namespace fcl { namespace implementation_array { template HierarchyTree::HierarchyTree(int bu_threshold_, int topdown_level_) { root_node = NULL_NODE; n_nodes = 0; n_nodes_alloc = 16; nodes = new NodeType[n_nodes_alloc]; for(size_t i = 0; i < n_nodes_alloc - 1; ++i) nodes[i].next = i + 1; nodes[n_nodes_alloc - 1].next = NULL_NODE; n_leaves = 0; freelist = 0; opath = 0; max_lookahead_level = -1; bu_threshold = bu_threshold_; topdown_level = topdown_level_; } template HierarchyTree::~HierarchyTree() { delete [] nodes; } template void HierarchyTree::init(NodeType* leaves, int n_leaves_, int level) { switch(level) { case 0: init_0(leaves, n_leaves_); break; case 1: init_1(leaves, n_leaves_); break; case 2: init_2(leaves, n_leaves_); break; case 3: init_3(leaves, n_leaves_); break; default: init_0(leaves, n_leaves_); } } template void HierarchyTree::init_0(NodeType* leaves, int n_leaves_) { clear(); n_leaves = n_leaves_; root_node = NULL_NODE; nodes = new NodeType[n_leaves * 2]; memcpy(nodes, leaves, sizeof(NodeType) * n_leaves); freelist = n_leaves; n_nodes = n_leaves; n_nodes_alloc = 2 * n_leaves; for(size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; nodes[n_nodes_alloc - 1].next = NULL_NODE; size_t* ids = new size_t[n_leaves]; for(size_t i = 0; i < n_leaves; ++i) ids[i] = i; root_node = topdown(ids, ids + n_leaves); delete [] ids; opath = 0; max_lookahead_level = -1; } template void HierarchyTree::init_1(NodeType* leaves, int n_leaves_) { clear(); n_leaves = n_leaves_; root_node = NULL_NODE; nodes = new NodeType[n_leaves * 2]; memcpy(nodes, leaves, sizeof(NodeType) * n_leaves); freelist = n_leaves; n_nodes = n_leaves; n_nodes_alloc = 2 * n_leaves; for(size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; nodes[n_nodes_alloc - 1].next = NULL_NODE; BV bound_bv; if(n_leaves > 0) bound_bv = nodes[0].bv; for(size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv; morton_functor coder(bound_bv); for(size_t i = 0; i < n_leaves; ++i) nodes[i].code = coder(nodes[i].bv.center()); size_t* ids = new size_t[n_leaves]; for(size_t i = 0; i < n_leaves; ++i) ids[i] = i; SortByMorton comp; comp.nodes = nodes; std::sort(ids, ids + n_leaves, comp); root_node = mortonRecurse_0(ids, ids + n_leaves, (1 << (coder.bits()-1)), coder.bits()-1); delete [] ids; refit(); opath = 0; max_lookahead_level = -1; } template void HierarchyTree::init_2(NodeType* leaves, int n_leaves_) { clear(); n_leaves = n_leaves_; root_node = NULL_NODE; nodes = new NodeType[n_leaves * 2]; memcpy(nodes, leaves, sizeof(NodeType) * n_leaves); freelist = n_leaves; n_nodes = n_leaves; n_nodes_alloc = 2 * n_leaves; for(size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; nodes[n_nodes_alloc - 1].next = NULL_NODE; BV bound_bv; if(n_leaves > 0) bound_bv = nodes[0].bv; for(size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv; morton_functor coder(bound_bv); for(size_t i = 0; i < n_leaves; ++i) nodes[i].code = coder(nodes[i].bv.center()); size_t* ids = new size_t[n_leaves]; for(size_t i = 0; i < n_leaves; ++i) ids[i] = i; SortByMorton comp; comp.nodes = nodes; std::sort(ids, ids + n_leaves, comp); root_node = mortonRecurse_1(ids, ids + n_leaves, (1 << (coder.bits()-1)), coder.bits()-1); delete [] ids; refit(); opath = 0; max_lookahead_level = -1; } template void HierarchyTree::init_3(NodeType* leaves, int n_leaves_) { clear(); n_leaves = n_leaves_; root_node = NULL_NODE; nodes = new NodeType[n_leaves * 2]; memcpy(nodes, leaves, sizeof(NodeType) * n_leaves); freelist = n_leaves; n_nodes = n_leaves; n_nodes_alloc = 2 * n_leaves; for(size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; nodes[n_nodes_alloc - 1].next = NULL_NODE; BV bound_bv; if(n_leaves > 0) bound_bv = nodes[0].bv; for(size_t i = 1; i < n_leaves; ++i) bound_bv += nodes[i].bv; morton_functor coder(bound_bv); for(size_t i = 0; i < n_leaves; ++i) nodes[i].code = coder(nodes[i].bv.center()); size_t* ids = new size_t[n_leaves]; for(size_t i = 0; i < n_leaves; ++i) ids[i] = i; SortByMorton comp; comp.nodes = nodes; std::sort(ids, ids + n_leaves, comp); root_node = mortonRecurse_2(ids, ids + n_leaves); delete [] ids; refit(); opath = 0; max_lookahead_level = -1; } template size_t HierarchyTree::insert(const BV& bv, void* data) { size_t node = createNode(NULL_NODE, bv, data); insertLeaf(root_node, node); ++n_leaves; return node; } template void HierarchyTree::remove(size_t leaf) { removeLeaf(leaf); deleteNode(leaf); --n_leaves; } template void HierarchyTree::clear() { delete [] nodes; root_node = NULL_NODE; n_nodes = 0; n_nodes_alloc = 16; nodes = new NodeType[n_nodes_alloc]; for(size_t i = 0; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; nodes[n_nodes_alloc - 1].next = NULL_NODE; n_leaves = 0; freelist = 0; opath = 0; max_lookahead_level = -1; } template bool HierarchyTree::empty() const { return (n_nodes == 0); } template void HierarchyTree::update(size_t leaf, int lookahead_level) { size_t root = removeLeaf(leaf); if(root != NULL_NODE) { if(lookahead_level > 0) { for(int i = 0; (i < lookahead_level) && (nodes[root].parent != NULL_NODE); ++i) root = nodes[root].parent; } else root = root_node; } insertLeaf(root, leaf); } template bool HierarchyTree::update(size_t leaf, const BV& bv) { if(nodes[leaf].bv.contain(bv)) return false; update_(leaf, bv); return true; } template bool HierarchyTree::update(size_t leaf, const BV& bv, const Vec3f& vel, FCL_REAL margin) { if(nodes[leaf].bv.contain(bv)) return false; update_(leaf, bv); return true; } template bool HierarchyTree::update(size_t leaf, const BV& bv, const Vec3f& vel) { if(nodes[leaf].bv.contain(bv)) return false; update_(leaf, bv); return true; } template size_t HierarchyTree::getMaxHeight() const { if(root_node == NULL_NODE) return 0; return getMaxHeight(root_node); } template size_t HierarchyTree::getMaxDepth() const { if(root_node == NULL_NODE) return 0; size_t max_depth; getMaxDepth(root_node, 0, max_depth); return max_depth; } template void HierarchyTree::balanceBottomup() { if(root_node != NULL_NODE) { NodeType* leaves = new NodeType[n_leaves]; NodeType* leaves_ = leaves; extractLeaves(root_node, leaves_); root_node = NULL_NODE; memcpy(nodes, leaves, sizeof(NodeType) * n_leaves); freelist = n_leaves; n_nodes = n_leaves; for(size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; nodes[n_nodes_alloc - 1].next = NULL_NODE; size_t* ids = new size_t[n_leaves]; for(size_t i = 0; i < n_leaves; ++i) ids[i] = i; bottomup(ids, ids + n_leaves); root_node = *ids; delete [] ids; } } template void HierarchyTree::balanceTopdown() { if(root_node != NULL_NODE) { NodeType* leaves = new NodeType[n_leaves]; NodeType* leaves_ = leaves; extractLeaves(root_node, leaves_); root_node = NULL_NODE; memcpy(nodes, leaves, sizeof(NodeType) * n_leaves); freelist = n_leaves; n_nodes = n_leaves; for(size_t i = n_leaves; i < n_nodes_alloc; ++i) nodes[i].next = i + 1; nodes[n_nodes_alloc - 1].next = NULL_NODE; size_t* ids = new size_t[n_leaves]; for(size_t i = 0; i < n_leaves; ++i) ids[i] = i; root_node = topdown(ids, ids + n_leaves); delete [] ids; } } template void HierarchyTree::balanceIncremental(int iterations) { if(iterations < 0) iterations = n_leaves; if((root_node != NULL_NODE) && (iterations > 0)) { for(int i = 0; i < iterations; ++i) { size_t node = root_node; unsigned int bit = 0; while(!nodes[node].isLeaf()) { node = nodes[node].children[(opath>>bit)&1]; bit = (bit+1)&(sizeof(unsigned int) * 8-1); } update(node); ++opath; } } } template void HierarchyTree::refit() { if(root_node != NULL_NODE) recurseRefit(root_node); } template void HierarchyTree::extractLeaves(size_t root, NodeType*& leaves) const { if(!nodes[root].isLeaf()) { extractLeaves(nodes[root].children[0], leaves); extractLeaves(nodes[root].children[1], leaves); } else { *leaves = nodes[root]; leaves++; } } template size_t HierarchyTree::size() const { return n_leaves; } template size_t HierarchyTree::getRoot() const { return root_node; } template typename HierarchyTree::NodeType* HierarchyTree::getNodes() const { return nodes; } template void HierarchyTree::print(size_t root, int depth) { for(int i = 0; i < depth; ++i) std::cout << " "; NodeType* n = nodes + root; std::cout << " (" << n->bv.min_[0] << ", " << n->bv.min_[1] << ", " << n->bv.min_[2] << "; " << n->bv.max_[0] << ", " << n->bv.max_[1] << ", " << n->bv.max_[2] << ")" << std::endl; if(n->isLeaf()) { } else { print(n->children[0], depth+1); print(n->children[1], depth+1); } } template size_t HierarchyTree::getMaxHeight(size_t node) const { if(!nodes[node].isLeaf()) { size_t height1 = getMaxHeight(nodes[node].children[0]); size_t height2 = getMaxHeight(nodes[node].children[1]); return std::max(height1, height2) + 1; } else return 0; } template void HierarchyTree::getMaxDepth(size_t node, size_t depth, size_t& max_depth) const { if(!nodes[node].isLeaf()) { getMaxDepth(nodes[node].children[0], depth+1, max_depth); getmaxDepth(nodes[node].children[1], depth+1, max_depth); } else max_depth = std::max(max_depth, depth); } template void HierarchyTree::bottomup(size_t* lbeg, size_t* lend) { size_t* lcur_end = lend; while(lbeg < lcur_end - 1) { size_t* min_it1 = NULL, *min_it2 = NULL; FCL_REAL min_size = std::numeric_limits::max(); for(size_t* it1 = lbeg; it1 < lcur_end; ++it1) { for(size_t* it2 = it1 + 1; it2 < lcur_end; ++it2) { FCL_REAL cur_size = (nodes[*it1].bv + nodes[*it2].bv).size(); if(cur_size < min_size) { min_size = cur_size; min_it1 = it1; min_it2 = it2; } } } size_t p = createNode(NULL_NODE, nodes[*min_it1].bv, nodes[*min_it2].bv, NULL); nodes[p].children[0] = *min_it1; nodes[p].children[1] = *min_it2; nodes[*min_it1].parent = p; nodes[*min_it2].parent = p; *min_it1 = p; size_t tmp = *min_it2; lcur_end--; *min_it2 = *lcur_end; *lcur_end = tmp; } } template size_t HierarchyTree::topdown(size_t* lbeg, size_t* lend) { switch(topdown_level) { case 0: return topdown_0(lbeg, lend); break; case 1: return topdown_1(lbeg, lend); break; default: return topdown_0(lbeg, lend); } } template size_t HierarchyTree::topdown_0(size_t* lbeg, size_t* lend) { int num_leaves = lend - lbeg; if(num_leaves > 1) { if(num_leaves > bu_threshold) { BV vol = nodes[*lbeg].bv; for(size_t* i = lbeg + 1; i < lend; ++i) vol += nodes[*i].bv; int best_axis = 0; FCL_REAL extent[3] = {vol.width(), vol.height(), vol.depth()}; if(extent[1] > extent[0]) best_axis = 1; if(extent[2] > extent[best_axis]) best_axis = 2; nodeBaseLess comp(nodes, best_axis); size_t* lcenter = lbeg + num_leaves / 2; std::nth_element(lbeg, lcenter, lend, comp); size_t node = createNode(NULL_NODE, vol, NULL); nodes[node].children[0] = topdown_0(lbeg, lcenter); nodes[node].children[1] = topdown_0(lcenter, lend); nodes[nodes[node].children[0]].parent = node; nodes[nodes[node].children[1]].parent = node; return node; } else { bottomup(lbeg, lend); return *lbeg; } } return *lbeg; } template size_t HierarchyTree::topdown_1(size_t* lbeg, size_t* lend) { int num_leaves = lend - lbeg; if(num_leaves > 1) { if(num_leaves > bu_threshold) { Vec3f split_p = nodes[*lbeg].bv.center(); BV vol = nodes[*lbeg].bv; for(size_t* i = lbeg + 1; i < lend; ++i) { split_p += nodes[*i].bv.center(); vol += nodes[*i].bv; } split_p /= (FCL_REAL)(num_leaves); int best_axis = -1; int bestmidp = num_leaves; int splitcount[3][2] = {{0,0}, {0,0}, {0,0}}; for(size_t* i = lbeg; i < lend; ++i) { Vec3f x = nodes[*i].bv.center() - split_p; for(size_t j = 0; j < 3; ++j) ++splitcount[j][x[j] > 0 ? 1 : 0]; } for(size_t i = 0; i < 3; ++i) { if((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) { int midp = std::abs(splitcount[i][0] - splitcount[i][1]); if(midp < bestmidp) { best_axis = i; bestmidp = midp; } } } if(best_axis < 0) best_axis = 0; FCL_REAL split_value = split_p[best_axis]; size_t* lcenter = lbeg; for(size_t* i = lbeg; i < lend; ++i) { if(nodes[*i].bv.center()[best_axis] < split_value) { size_t temp = *i; *i = *lcenter; *lcenter = temp; ++lcenter; } } size_t node = createNode(NULL_NODE, vol, NULL); nodes[node].children[0] = topdown_1(lbeg, lcenter); nodes[node].children[1] = topdown_1(lcenter, lend); nodes[nodes[node].children[0]].parent = node; nodes[nodes[node].children[1]].parent = node; return node; } else { bottomup(lbeg, lend); return *lbeg; } } return *lbeg; } template size_t HierarchyTree::mortonRecurse_0(size_t* lbeg, size_t* lend, const FCL_UINT32& split, int bits) { int num_leaves = lend - lbeg; if(num_leaves > 1) { if(bits > 0) { SortByMorton comp; comp.nodes = nodes; comp.split = split; size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp); if(lcenter == lbeg) { FCL_UINT32 split2 = split | (1 << (bits - 1)); return mortonRecurse_0(lbeg, lend, split2, bits - 1); } else if(lcenter == lend) { FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); return mortonRecurse_0(lbeg, lend, split1, bits - 1); } else { FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); FCL_UINT32 split2 = split | (1 << (bits - 1)); size_t child1 = mortonRecurse_0(lbeg, lcenter, split1, bits - 1); size_t child2 = mortonRecurse_0(lcenter, lend, split2, bits - 1); size_t node = createNode(NULL_NODE, NULL); nodes[node].children[0] = child1; nodes[node].children[1] = child2; nodes[child1].parent = node; nodes[child2].parent = node; return node; } } else { size_t node = topdown(lbeg, lend); return node; } } else return *lbeg; } template size_t HierarchyTree::mortonRecurse_1(size_t* lbeg, size_t* lend, const FCL_UINT32& split, int bits) { int num_leaves = lend - lbeg; if(num_leaves > 1) { if(bits > 0) { SortByMorton comp; comp.nodes = nodes; comp.split = split; size_t* lcenter = std::lower_bound(lbeg, lend, NULL_NODE, comp); if(lcenter == lbeg) { FCL_UINT32 split2 = split | (1 << (bits - 1)); return mortonRecurse_1(lbeg, lend, split2, bits - 1); } else if(lcenter == lend) { FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); return mortonRecurse_1(lbeg, lend, split1, bits - 1); } else { FCL_UINT32 split1 = (split & (~(1 << bits))) | (1 << (bits - 1)); FCL_UINT32 split2 = split | (1 << (bits - 1)); size_t child1 = mortonRecurse_1(lbeg, lcenter, split1, bits - 1); size_t child2 = mortonRecurse_1(lcenter, lend, split2, bits - 1); size_t node = createNode(NULL_NODE, NULL); nodes[node].children[0] = child1; nodes[node].children[1] = child2; nodes[child1].parent = node; nodes[child2].parent = node; return node; } } else { size_t child1 = mortonRecurse_1(lbeg, lbeg + num_leaves / 2, 0, bits - 1); size_t child2 = mortonRecurse_1(lbeg + num_leaves / 2, lend, 0, bits - 1); size_t node = createNode(NULL_NODE, NULL); nodes[node].children[0] = child1; nodes[node].children[1] = child2; nodes[child1].parent = node; nodes[child2].parent = node; return node; } } else return *lbeg; } template size_t HierarchyTree::mortonRecurse_2(size_t* lbeg, size_t* lend) { int num_leaves = lend - lbeg; if(num_leaves > 1) { size_t child1 = mortonRecurse_2(lbeg, lbeg + num_leaves / 2); size_t child2 = mortonRecurse_2(lbeg + num_leaves / 2, lend); size_t node = createNode(NULL_NODE, NULL); nodes[node].children[0] = child1; nodes[node].children[1] = child2; nodes[child1].parent = node; nodes[child2].parent = node; return node; } else return *lbeg; } template void HierarchyTree::insertLeaf(size_t root, size_t leaf) { if(root_node == NULL_NODE) { root_node = leaf; nodes[leaf].parent = NULL_NODE; } else { if(!nodes[root].isLeaf()) { do { root = nodes[root].children[select(leaf, nodes[root].children[0], nodes[root].children[1], nodes)]; } while(!nodes[root].isLeaf()); } size_t prev = nodes[root].parent; size_t node = createNode(prev, nodes[leaf].bv, nodes[root].bv, NULL); if(prev != NULL_NODE) { nodes[prev].children[indexOf(root)] = node; nodes[node].children[0] = root; nodes[root].parent = node; nodes[node].children[1] = leaf; nodes[leaf].parent = node; do { if(!nodes[prev].bv.contain(nodes[node].bv)) nodes[prev].bv = nodes[nodes[prev].children[0]].bv + nodes[nodes[prev].children[1]].bv; else break; node = prev; } while (NULL_NODE != (prev = nodes[node].parent)); } else { nodes[node].children[0] = root; nodes[root].parent = node; nodes[node].children[1] = leaf; nodes[leaf].parent = node; root_node = node; } } } template size_t HierarchyTree::removeLeaf(size_t leaf) { if(leaf == root_node) { root_node = NULL_NODE; return NULL_NODE; } else { size_t parent = nodes[leaf].parent; size_t prev = nodes[parent].parent; size_t sibling = nodes[parent].children[1-indexOf(leaf)]; if(prev != NULL_NODE) { nodes[prev].children[indexOf(parent)] = sibling; nodes[sibling].parent = prev; deleteNode(parent); while(prev != NULL_NODE) { BV new_bv = nodes[nodes[prev].children[0]].bv + nodes[nodes[prev].children[1]].bv; if(!new_bv.equal(nodes[prev].bv)) { nodes[prev].bv = new_bv; prev = nodes[prev].parent; } else break; } return (prev != NULL_NODE) ? prev : root_node; } else { root_node = sibling; nodes[sibling].parent = NULL_NODE; deleteNode(parent); return root_node; } } } template void HierarchyTree::update_(size_t leaf, const BV& bv) { size_t root = removeLeaf(leaf); if(root != NULL_NODE) { if(max_lookahead_level >= 0) { for(int i = 0; (i < max_lookahead_level) && (nodes[root].parent != NULL_NODE); ++i) root = nodes[root].parent; } nodes[leaf].bv = bv; insertLeaf(root, leaf); } } template size_t HierarchyTree::indexOf(size_t node) { return (nodes[nodes[node].parent].children[1] == node); } template size_t HierarchyTree::allocateNode() { if(freelist == NULL_NODE) { NodeType* old_nodes = nodes; n_nodes_alloc *= 2; nodes = new NodeType[n_nodes_alloc]; memcpy(nodes, old_nodes, n_nodes * sizeof(NodeType)); delete [] old_nodes; for(size_t i = n_nodes; i < n_nodes_alloc - 1; ++i) nodes[i].next = i + 1; nodes[n_nodes_alloc - 1].next = NULL_NODE; freelist = n_nodes; } size_t node_id = freelist; freelist = nodes[node_id].next; nodes[node_id].parent = NULL_NODE; nodes[node_id].children[0] = NULL_NODE; nodes[node_id].children[1] = NULL_NODE; ++n_nodes; return node_id; } template size_t HierarchyTree::createNode(size_t parent, const BV& bv1, const BV& bv2, void* data) { size_t node = allocateNode(); nodes[node].parent = parent; nodes[node].data = data; nodes[node].bv = bv1 + bv2; return node; } template size_t HierarchyTree::createNode(size_t parent, const BV& bv, void* data) { size_t node = allocateNode(); nodes[node].parent = parent; nodes[node].data = data; nodes[node].bv = bv; return node; } template size_t HierarchyTree::createNode(size_t parent, void* data) { size_t node = allocateNode(); nodes[node].parent = parent; nodes[node].data = data; return node; } template void HierarchyTree::deleteNode(size_t node) { nodes[node].next = freelist; freelist = node; --n_nodes; } template void HierarchyTree::recurseRefit(size_t node) { if(!nodes[node].isLeaf()) { recurseRefit(nodes[node].children[0]); recurseRefit(nodes[node].children[1]); nodes[node].bv = nodes[nodes[node].children[0]].bv + nodes[nodes[node].children[1]].bv; } else return; } template void HierarchyTree::fetchLeaves(size_t root, NodeType*& leaves, int depth) { if((!nodes[root].isLeaf()) && depth) { fetchLeaves(nodes[root].children[0], leaves, depth-1); fetchLeaves(nodes[root].children[1], leaves, depth-1); deleteNode(root); } else { *leaves = nodes[root]; leaves++; } } } } fcl-0.5.0/include/fcl/broadphase/interval_tree.h000066400000000000000000000111541274356571000216070ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_INTERVAL_TREE_H #define FCL_INTERVAL_TREE_H #include #include namespace fcl { /// @brief Interval trees implemented using red-black-trees as described in /// the book Introduction_To_Algorithms_ by Cormen, Leisserson, and Rivest. struct SimpleInterval { public: virtual ~SimpleInterval() {} virtual void print() {} /// @brief interval is defined as [low, high] double low, high; }; /// @brief The node for interval tree class IntervalTreeNode { friend class IntervalTree; public: /// @brief Print the interval node information: set left = nil and right = root void print(IntervalTreeNode* left, IntervalTreeNode* right) const; /// @brief Create an empty node IntervalTreeNode(); /// @brief Create an node storing the interval IntervalTreeNode(SimpleInterval* new_interval); ~IntervalTreeNode(); protected: /// @brief interval stored in the node SimpleInterval* stored_interval; double key; double high; double max_high; /// @brief red or black node: if red = false then the node is black bool red; IntervalTreeNode* left; IntervalTreeNode* right; IntervalTreeNode* parent; }; struct it_recursion_node; /// @brief Interval tree class IntervalTree { public: IntervalTree(); ~IntervalTree(); /// @brief Print the whole interval tree void print() const; /// @brief Delete one node of the interval tree SimpleInterval* deleteNode(IntervalTreeNode* node); /// @brief delete node stored a given interval void deleteNode(SimpleInterval* ivl); /// @brief Insert one node of the interval tree IntervalTreeNode* insert(SimpleInterval* new_interval); /// @brief get the predecessor of a given node IntervalTreeNode* getPredecessor(IntervalTreeNode* node) const; /// @brief Get the successor of a given node IntervalTreeNode* getSuccessor(IntervalTreeNode* node) const; /// @brief Return result for a given query std::deque query(double low, double high); protected: IntervalTreeNode* root; IntervalTreeNode* nil; /// @brief left rotation of tree node void leftRotate(IntervalTreeNode* node); /// @brief right rotation of tree node void rightRotate(IntervalTreeNode* node); /// @brief recursively insert a node void recursiveInsert(IntervalTreeNode* node); /// @brief recursively print a subtree void recursivePrint(IntervalTreeNode* node) const; /// @brief recursively find the node corresponding to the interval IntervalTreeNode* recursiveSearch(IntervalTreeNode* node, SimpleInterval* ivl) const; /// @brief Travels up to the root fixing the max_high fields after an insertion or deletion void fixupMaxHigh(IntervalTreeNode* node); void deleteFixup(IntervalTreeNode* node); private: unsigned int recursion_node_stack_size; it_recursion_node* recursion_node_stack; unsigned int current_parent; unsigned int recursion_node_stack_top; }; } #endif fcl-0.5.0/include/fcl/broadphase/morton.h000066400000000000000000000144151274356571000202650ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * Copyright (c) 2016, Toyota Research Institute * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_MORTON_H #define FCL_MORTON_H #include "fcl/data_types.h" #include "fcl/BV/AABB.h" #include namespace fcl { /// @cond IGNORE namespace details { static inline FCL_UINT32 quantize(FCL_REAL x, FCL_UINT32 n) { return std::max(std::min((FCL_UINT32)(x * (FCL_REAL)n), FCL_UINT32(n-1)), FCL_UINT32(0)); } /// @brief compute 30 bit morton code static inline FCL_UINT32 morton_code(FCL_UINT32 x, FCL_UINT32 y, FCL_UINT32 z) { x = (x | (x << 16)) & 0x030000FF; x = (x | (x << 8)) & 0x0300F00F; x = (x | (x << 4)) & 0x030C30C3; x = (x | (x << 2)) & 0x09249249; y = (y | (y << 16)) & 0x030000FF; y = (y | (y << 8)) & 0x0300F00F; y = (y | (y << 4)) & 0x030C30C3; y = (y | (y << 2)) & 0x09249249; z = (z | (z << 16)) & 0x030000FF; z = (z | (z << 8)) & 0x0300F00F; z = (z | (z << 4)) & 0x030C30C3; z = (z | (z << 2)) & 0x09249249; return x | (y << 1) | (z << 2); } /// @brief compute 60 bit morton code static inline FCL_UINT64 morton_code60(FCL_UINT32 x, FCL_UINT32 y, FCL_UINT32 z) { FCL_UINT32 lo_x = x & 1023u; FCL_UINT32 lo_y = y & 1023u; FCL_UINT32 lo_z = z & 1023u; FCL_UINT32 hi_x = x >> 10u; FCL_UINT32 hi_y = y >> 10u; FCL_UINT32 hi_z = z >> 10u; return (FCL_UINT64(morton_code(hi_x, hi_y, hi_z)) << 30) | FCL_UINT64(morton_code(lo_x, lo_y, lo_z)); } } /// @endcond /// @brief Functor to compute the morton code for a given AABB /// This is specialized for 32- and 64-bit unsigned integers giving /// a 30- or 60-bit code, respectively, and for `std::bitset` where /// N is the length of the code and must be a multiple of 3. template struct morton_functor {}; /// @brief Functor to compute 30 bit morton code for a given AABB template<> struct morton_functor { morton_functor(const AABB& bbox) : base(bbox.min_), inv(1.0 / (bbox.max_[0] - bbox.min_[0]), 1.0 / (bbox.max_[1] - bbox.min_[1]), 1.0 / (bbox.max_[2] - bbox.min_[2])) {} FCL_UINT32 operator() (const Vec3f& point) const { FCL_UINT32 x = details::quantize((point[0] - base[0]) * inv[0], 1024u); FCL_UINT32 y = details::quantize((point[1] - base[1]) * inv[1], 1024u); FCL_UINT32 z = details::quantize((point[2] - base[2]) * inv[2], 1024u); return details::morton_code(x, y, z); } const Vec3f base; const Vec3f inv; static constexpr size_t bits() { return 30; } }; /// @brief Functor to compute 60 bit morton code for a given AABB template<> struct morton_functor { morton_functor(const AABB& bbox) : base(bbox.min_), inv(1.0 / (bbox.max_[0] - bbox.min_[0]), 1.0 / (bbox.max_[1] - bbox.min_[1]), 1.0 / (bbox.max_[2] - bbox.min_[2])) {} FCL_UINT64 operator() (const Vec3f& point) const { FCL_UINT32 x = details::quantize((point[0] - base[0]) * inv[0], 1u << 20); FCL_UINT32 y = details::quantize((point[1] - base[1]) * inv[1], 1u << 20); FCL_UINT32 z = details::quantize((point[2] - base[2]) * inv[2], 1u << 20); return details::morton_code60(x, y, z); } const Vec3f base; const Vec3f inv; static constexpr size_t bits() { return 60; } }; /// @brief Functor to compute N bit morton code for a given AABB /// N must be a multiple of 3. template struct morton_functor> { static_assert(N%3==0, "Number of bits must be a multiple of 3"); morton_functor(const AABB& bbox) : base(bbox.min_), inv(1.0 / (bbox.max_[0] - bbox.min_[0]), 1.0 / (bbox.max_[1] - bbox.min_[1]), 1.0 / (bbox.max_[2] - bbox.min_[2])) {} std::bitset operator() (const Vec3f& point) const { FCL_REAL x = (point[0] - base[0]) * inv[0]; FCL_REAL y = (point[1] - base[1]) * inv[1]; FCL_REAL z = (point[2] - base[2]) * inv[2]; int start_bit = bits() - 1; std::bitset bset; x *= 2; y *= 2; z *= 2; for(size_t i = 0; i < bits()/3; ++i) { bset[start_bit--] = ((z < 1) ? 0 : 1); bset[start_bit--] = ((y < 1) ? 0 : 1); bset[start_bit--] = ((x < 1) ? 0 : 1); x = ((x >= 1) ? 2*(x-1) : 2*x); y = ((y >= 1) ? 2*(y-1) : 2*y); z = ((z >= 1) ? 2*(z-1) : 2*z); } return bset; } const Vec3f base; const Vec3f inv; static constexpr size_t bits() { return N; } }; } #endif fcl-0.5.0/include/fcl/ccd/000077500000000000000000000000001274356571000152125ustar00rootroot00000000000000fcl-0.5.0/include/fcl/ccd/conservative_advancement.h000066400000000000000000000046601274356571000224460ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_CONSERVATIVE_ADVANCEMENT_H #define FCL_CONSERVATIVE_ADVANCEMENT_H #include "fcl/collision_object.h" #include "fcl/collision_data.h" #include "fcl/ccd/motion_base.h" namespace fcl { template struct ConservativeAdvancementFunctionMatrix { typedef FCL_REAL (*ConservativeAdvancementFunc)(const CollisionGeometry* o1, const MotionBase* motion1, const CollisionGeometry* o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result); ConservativeAdvancementFunc conservative_advancement_matrix[NODE_COUNT][NODE_COUNT]; ConservativeAdvancementFunctionMatrix(); }; } #endif fcl-0.5.0/include/fcl/ccd/interpolation/000077500000000000000000000000001274356571000201015ustar00rootroot00000000000000fcl-0.5.0/include/fcl/ccd/interpolation/interpolation.h000066400000000000000000000055371274356571000231530ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Dalibor Matura, Jia Pan */ #ifndef FCL_CCD_INTERPOLATION_INTERPOLATION_H #define FCL_CCD_INTERPOLATION_INTERPOLATION_H #include "fcl/data_types.h" namespace fcl { enum InterpolationType { LINEAR, STANDARD }; class Interpolation { public: Interpolation(); virtual ~Interpolation() {} Interpolation(FCL_REAL start_value, FCL_REAL end_value); void setStartValue(FCL_REAL start_value); void setEndValue(FCL_REAL end_value); virtual FCL_REAL getValue(FCL_REAL time) const = 0; /// @brief return the smallest value in time interval [0, 1] virtual FCL_REAL getValueLowerBound() const = 0; /// @brief return the biggest value in time interval [0, 1] virtual FCL_REAL getValueUpperBound() const = 0; virtual InterpolationType getType() const = 0; bool operator == (const Interpolation& interpolation) const; bool operator != (const Interpolation& interpolation) const; virtual FCL_REAL getMovementLengthBound(FCL_REAL time) const = 0; virtual FCL_REAL getVelocityBound(FCL_REAL time) const = 0; protected: FCL_REAL value_0_; // value at time = 0.0 FCL_REAL value_1_; // value at time = 1.0 }; } #endif fcl-0.5.0/include/fcl/ccd/interpolation/interpolation_factory.h000066400000000000000000000052331274356571000246730ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Dalibor Matura, Jia Pan */ #ifndef FCL_CCD_INTERPOLATION_INTERPOLATION_FACTORY_H #define FCL_CCD_INTERPOLATION_INTERPOLATION_FACTORY_H #include "fcl/data_types.h" #include "fcl/ccd/interpolation/interpolation.h" #include #include #include namespace fcl { class InterpolationFactory { public: typedef std::function(FCL_REAL, FCL_REAL)> CreateFunction; public: void registerClass(const InterpolationType type, const CreateFunction create_function); std::shared_ptr create(const InterpolationType type, FCL_REAL start_value, FCL_REAL end_value); public: static InterpolationFactory& instance(); private: InterpolationFactory(); InterpolationFactory(const InterpolationFactory&) {} InterpolationFactory& operator = (const InterpolationFactory&) { return *this; } private: std::map creation_map_; }; } #endif fcl-0.5.0/include/fcl/ccd/interpolation/interpolation_linear.h000066400000000000000000000051351274356571000244770ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Dalibor Matura, Jia Pan */ #ifndef FCL_CCD_INTERPOLATION_INTERPOLATION_LINEAR_H #define FCL_CCD_INTERPOLATION_INTERPOLATION_LINEAR_H #include "fcl/data_types.h" #include "fcl/ccd/interpolation/interpolation.h" #include namespace fcl { class InterpolationFactory; class InterpolationLinear : public Interpolation { public: InterpolationLinear(); InterpolationLinear(FCL_REAL start_value, FCL_REAL end_value); virtual FCL_REAL getValue(FCL_REAL time) const; virtual FCL_REAL getValueLowerBound() const; virtual FCL_REAL getValueUpperBound() const; virtual InterpolationType getType() const; virtual FCL_REAL getMovementLengthBound(FCL_REAL time) const; virtual FCL_REAL getVelocityBound(FCL_REAL time) const; public: static std::shared_ptr create(FCL_REAL start_value, FCL_REAL end_value); static void registerToFactory(); }; } #endif fcl-0.5.0/include/fcl/ccd/interval.h000066400000000000000000000137271274356571000172210ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ // This code is based on code developed by Stephane Redon at UNC and Inria for the CATCH library: http://graphics.ewha.ac.kr/CATCH/ /** \author Jia Pan */ #ifndef FCL_CCD_INTERVAL_H #define FCL_CCD_INTERVAL_H #include "fcl/data_types.h" namespace fcl { /// @brief Interval class for [a, b] struct Interval { FCL_REAL i_[2]; Interval() { i_[0] = i_[1] = 0; } explicit Interval(FCL_REAL v) { i_[0] = i_[1] = v; } /// @brief construct interval [left, right] Interval(FCL_REAL left, FCL_REAL right) { i_[0] = left; i_[1] = right; } /// @brief construct interval [left, right] inline void setValue(FCL_REAL a, FCL_REAL b) { i_[0] = a; i_[1] = b; } /// @brief construct zero interval [x, x] inline void setValue(FCL_REAL x) { i_[0] = i_[1] = x; } /// @brief access the interval endpoints: 0 for left, 1 for right end inline FCL_REAL operator [] (size_t i) const { return i_[i]; } /// @brief access the interval endpoints: 0 for left, 1 for right end inline FCL_REAL& operator [] (size_t i) { return i_[i]; } /// @brief whether two intervals are the same inline bool operator == (const Interval& other) const { if(i_[0] != other.i_[0]) return false; if(i_[1] != other.i_[1]) return false; return true; } /// @brief add two intervals inline Interval operator + (const Interval& other) const { return Interval(i_[0] + other.i_[0], i_[1] + other.i_[1]); } /// @brief minus another interval inline Interval operator - (const Interval& other) const { return Interval(i_[0] - other.i_[1], i_[1] - other.i_[0]); } inline Interval& operator += (const Interval& other) { i_[0] += other.i_[0]; i_[1] += other.i_[1]; return *this; } inline Interval& operator -= (const Interval& other) { i_[0] -= other.i_[1]; i_[1] -= other.i_[0]; return *this; } Interval operator * (const Interval& other) const; Interval& operator *= (const Interval& other); inline Interval operator * (FCL_REAL d) const { if(d >= 0) return Interval(i_[0] * d, i_[1] * d); return Interval(i_[1] * d, i_[0] * d); } inline Interval& operator *= (FCL_REAL d) { if(d >= 0) { i_[0] *= d; i_[1] *= d; } else { FCL_REAL tmp = i_[0]; i_[0] = i_[1] * d; i_[1] = tmp * d; } return *this; } /// @brief other must not contain 0 Interval operator / (const Interval& other) const; Interval& operator /= (const Interval& other); /// @brief determine whether the intersection between intervals is empty inline bool overlap(const Interval& other) const { if(i_[1] < other.i_[0]) return false; if(i_[0] > other.i_[1]) return false; return true; } inline bool intersect(const Interval& other) { if(i_[1] < other.i_[0]) return false; if(i_[0] > other.i_[1]) return false; if(i_[1] > other.i_[1]) i_[1] = other.i_[1]; if(i_[0] < other.i_[0]) i_[0] = other.i_[0]; return true; } inline Interval operator - () const { return Interval(-i_[1], -i_[0]); } /// @brief Return the nearest distance for points within the interval to zero inline FCL_REAL getAbsLower() const { if(i_[0] >= 0) return i_[0]; if(i_[1] >= 0) return 0; return -i_[1]; } /// @brief Return the farthest distance for points within the interval to zero inline FCL_REAL getAbsUpper() const { if(i_[0] + i_[1] >= 0) return i_[1]; return i_[0]; } inline bool contains(FCL_REAL v) const { if(v < i_[0]) return false; if(v > i_[1]) return false; return true; } /// @brief Compute the minimum interval contains v and original interval inline Interval& bound(FCL_REAL v) { if(v < i_[0]) i_[0] = v; if(v > i_[1]) i_[1] = v; return *this; } /// @brief Compute the minimum interval contains other and original interval inline Interval& bound(const Interval& other) { if(other.i_[0] < i_[0]) i_[0] = other.i_[0]; if(other.i_[1] > i_[1]) i_[1] = other.i_[1]; return *this; } void print() const; inline FCL_REAL center() const { return 0.5 * (i_[0] + i_[1]); } inline FCL_REAL diameter() const { return i_[1] -i_[0]; } }; Interval bound(const Interval& i, FCL_REAL v); Interval bound(const Interval& i, const Interval& other); } #endif fcl-0.5.0/include/fcl/ccd/interval_matrix.h000066400000000000000000000065601274356571000206020ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ // This code is based on code developed by Stephane Redon at UNC and Inria for the CATCH library: http://graphics.ewha.ac.kr/CATCH/ /** \author Jia Pan */ #ifndef FCL_CCD_INTERVAL_MATRIX_H #define FCL_CCD_INTERVAL_MATRIX_H #include "fcl/ccd/interval.h" #include "fcl/ccd/interval_vector.h" #include "fcl/math/matrix_3f.h" namespace fcl { struct IMatrix3 { IVector3 v_[3]; IMatrix3(); IMatrix3(FCL_REAL v); IMatrix3(const Matrix3f& m); IMatrix3(FCL_REAL m[3][3][2]); IMatrix3(FCL_REAL m[3][3]); IMatrix3(Interval m[3][3]); IMatrix3(const IVector3& v1, const IVector3& v2, const IVector3& v3); void setIdentity(); IVector3 getColumn(size_t i) const; const IVector3& getRow(size_t i) const; Vec3f getColumnLow(size_t i) const; Vec3f getRowLow(size_t i) const; Vec3f getColumnHigh(size_t i) const; Vec3f getRowHigh(size_t i) const; Matrix3f getLow() const; Matrix3f getHigh() const; inline const Interval& operator () (size_t i, size_t j) const { return v_[i][j]; } inline Interval& operator () (size_t i, size_t j) { return v_[i][j]; } IMatrix3 operator + (const IMatrix3& m) const; IMatrix3& operator += (const IMatrix3& m); IMatrix3 operator - (const IMatrix3& m) const; IMatrix3& operator -= (const IMatrix3& m); IVector3 operator * (const Vec3f& v) const; IVector3 operator * (const IVector3& v) const; IMatrix3 operator * (const IMatrix3& m) const; IMatrix3 operator * (const Matrix3f& m) const; IMatrix3& operator *= (const IMatrix3& m); IMatrix3& operator *= (const Matrix3f& m); IMatrix3& rotationConstrain(); void print() const; }; IMatrix3 rotationConstrain(const IMatrix3& m); } #endif fcl-0.5.0/include/fcl/ccd/interval_vector.h000066400000000000000000000107631274356571000206000ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ // This code is based on code developed by Stephane Redon at UNC and Inria for the CATCH library: http://graphics.ewha.ac.kr/CATCH/ /** \author Jia Pan */ #ifndef FCL_CCD_INTERVAL_VECTOR_H #define FCL_CCD_INTERVAL_VECTOR_H #include "fcl/ccd/interval.h" #include "fcl/math/vec_3f.h" namespace fcl { struct IVector3 { Interval i_[3]; IVector3(); IVector3(FCL_REAL v); IVector3(FCL_REAL x, FCL_REAL y, FCL_REAL z); IVector3(FCL_REAL xl, FCL_REAL xu, FCL_REAL yl, FCL_REAL yu, FCL_REAL zl, FCL_REAL zu); IVector3(Interval v[3]); IVector3(FCL_REAL v[3][2]); IVector3(const Interval& v1, const Interval& v2, const Interval& v3); IVector3(const Vec3f& v); inline void setValue(FCL_REAL v) { i_[0].setValue(v); i_[1].setValue(v); i_[2].setValue(v); } inline void setValue(FCL_REAL x, FCL_REAL y, FCL_REAL z) { i_[0].setValue(x); i_[1].setValue(y); i_[2].setValue(z); } inline void setValue(FCL_REAL xl, FCL_REAL xu, FCL_REAL yl, FCL_REAL yu, FCL_REAL zl, FCL_REAL zu) { i_[0].setValue(xl, xu); i_[1].setValue(yl, yu); i_[2].setValue(zl, zu); } inline void setValue(FCL_REAL v[3][2]) { i_[0].setValue(v[0][0], v[0][1]); i_[1].setValue(v[1][0], v[1][1]); i_[2].setValue(v[2][0], v[2][1]); } inline void setValue(Interval v[3]) { i_[0] = v[0]; i_[1] = v[1]; i_[2] = v[2]; } inline void setValue(const Interval& v1, const Interval& v2, const Interval& v3) { i_[0] = v1; i_[1] = v2; i_[2] = v3; } inline void setValue(const Vec3f& v) { i_[0].setValue(v[0]); i_[1].setValue(v[1]); i_[2].setValue(v[2]); } inline void setValue(FCL_REAL v[3]) { i_[0].setValue(v[0]); i_[1].setValue(v[1]); i_[2].setValue(v[2]); } IVector3 operator + (const IVector3& other) const; IVector3& operator += (const IVector3& other); IVector3 operator - (const IVector3& other) const; IVector3& operator -= (const IVector3& other); Interval dot(const IVector3& other) const; IVector3 cross(const IVector3& other) const; Interval dot(const Vec3f& other) const; IVector3 cross(const Vec3f& other) const; inline const Interval& operator [] (size_t i) const { return i_[i]; } inline Interval& operator [] (size_t i) { return i_[i]; } inline Vec3f getLow() const { return Vec3f(i_[0][0], i_[1][0], i_[2][0]); } inline Vec3f getHigh() const { return Vec3f(i_[0][1], i_[1][1], i_[2][1]); } void print() const; Vec3f center() const; FCL_REAL volumn() const; void setZero(); void bound(const Vec3f& v); void bound(const IVector3& v); bool overlap(const IVector3& v) const; bool contain(const IVector3& v) const; }; IVector3 bound(const IVector3& i, const Vec3f& v); IVector3 bound(const IVector3& i, const IVector3& v); } #endif fcl-0.5.0/include/fcl/ccd/motion.h000066400000000000000000000414351274356571000166770ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_CCD_MOTION_H #define FCL_CCD_MOTION_H #include "fcl/ccd/motion_base.h" #include "fcl/intersect.h" #include #include namespace fcl { class TranslationMotion : public MotionBase { public: /// @brief Construct motion from intial and goal transform TranslationMotion(const Transform3f& tf1, const Transform3f& tf2) : MotionBase(), rot(tf1.getQuatRotation()), trans_start(tf1.getTranslation()), trans_range(tf2.getTranslation() - tf1.getTranslation()), tf(tf1) { } TranslationMotion(const Matrix3f& R, const Vec3f& T1, const Vec3f& T2) : MotionBase() { rot.fromRotation(R); trans_start = T1; trans_range = T2 - T1; tf = Transform3f(rot, trans_start); } bool integrate(FCL_REAL dt) const { if(dt > 1) dt = 1; tf = Transform3f(rot, trans_start + trans_range * dt); return true; } FCL_REAL computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const { return mb_visitor.visit(*this); } FCL_REAL computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const { return mb_visitor.visit(*this); } void getCurrentTransform(Transform3f& tf_) const { tf_ = tf; } void getTaylorModel(TMatrix3& tm, TVector3& tv) const { } Vec3f getVelocity() const { return trans_range; } private: /// @brief initial and goal transforms Quaternion3f rot; Vec3f trans_start, trans_range; mutable Transform3f tf; }; class SplineMotion : public MotionBase { public: /// @brief Construct motion from 4 deBoor points SplineMotion(const Vec3f& Td0, const Vec3f& Td1, const Vec3f& Td2, const Vec3f& Td3, const Vec3f& Rd0, const Vec3f& Rd1, const Vec3f& Rd2, const Vec3f& Rd3); // @brief Construct motion from initial and goal transform SplineMotion(const Matrix3f& R1, const Vec3f& T1, const Matrix3f& R2, const Vec3f& T2) : MotionBase() { // TODO } SplineMotion(const Transform3f& tf1, const Transform3f& tf2) : MotionBase() { // TODO } /// @brief Integrate the motion from 0 to dt /// We compute the current transformation from zero point instead of from last integrate time, for precision. bool integrate(double dt) const; /// @brief Compute the motion bound for a bounding volume along a given direction n, which is defined in the visitor FCL_REAL computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const { return mb_visitor.visit(*this); } /// @brief Compute the motion bound for a triangle along a given direction n, which is defined in the visitor FCL_REAL computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const { return mb_visitor.visit(*this); } /// @brief Get the rotation and translation in current step void getCurrentTransform(Transform3f& tf_) const { tf_ = tf; } void getTaylorModel(TMatrix3& tm, TVector3& tv) const { // set tv Vec3f c[4]; c[0] = (Td[0] + Td[1] * 4 + Td[2] + Td[3]) * (1/6.0); c[1] = (-Td[0] + Td[2]) * (1/2.0); c[2] = (Td[0] - Td[1] * 2 + Td[2]) * (1/2.0); c[3] = (-Td[0] + Td[1] * 3 - Td[2] * 3 + Td[3]) * (1/6.0); tv.setTimeInterval(getTimeInterval()); for(std::size_t i = 0; i < 3; ++i) { for(std::size_t j = 0; j < 4; ++j) { tv[i].coeff(j) = c[j][i]; } } // set tm Matrix3f I(1, 0, 0, 0, 1, 0, 0, 0, 1); // R(t) = R(t0) + R'(t0) (t-t0) + 1/2 R''(t0)(t-t0)^2 + 1 / 6 R'''(t0) (t-t0)^3 + 1 / 24 R''''(l)(t-t0)^4; t0 = 0.5 /// 1. compute M(1/2) Vec3f Rt0 = (Rd[0] + Rd[1] * 23 + Rd[2] * 23 + Rd[3]) * (1 / 48.0); FCL_REAL Rt0_len = Rt0.length(); FCL_REAL inv_Rt0_len = 1.0 / Rt0_len; FCL_REAL inv_Rt0_len_3 = inv_Rt0_len * inv_Rt0_len * inv_Rt0_len; FCL_REAL inv_Rt0_len_5 = inv_Rt0_len_3 * inv_Rt0_len * inv_Rt0_len; FCL_REAL theta0 = Rt0_len; FCL_REAL costheta0 = cos(theta0); FCL_REAL sintheta0 = sin(theta0); Vec3f Wt0 = Rt0 * inv_Rt0_len; Matrix3f hatWt0; hat(hatWt0, Wt0); Matrix3f hatWt0_sqr = hatWt0 * hatWt0; Matrix3f Mt0 = I + hatWt0 * sintheta0 + hatWt0_sqr * (1 - costheta0); /// 2. compute M'(1/2) Vec3f dRt0 = (-Rd[0] - Rd[1] * 5 + Rd[2] * 5 + Rd[3]) * (1 / 8.0); FCL_REAL Rt0_dot_dRt0 = Rt0.dot(dRt0); FCL_REAL dtheta0 = Rt0_dot_dRt0 * inv_Rt0_len; Vec3f dWt0 = dRt0 * inv_Rt0_len - Rt0 * (Rt0_dot_dRt0 * inv_Rt0_len_3); Matrix3f hatdWt0; hat(hatdWt0, dWt0); Matrix3f dMt0 = hatdWt0 * sintheta0 + hatWt0 * (costheta0 * dtheta0) + hatWt0_sqr * (sintheta0 * dtheta0) + (hatWt0 * hatdWt0 + hatdWt0 * hatWt0) * (1 - costheta0); /// 3.1. compute M''(1/2) Vec3f ddRt0 = (Rd[0] - Rd[1] - Rd[2] + Rd[3]) * 0.5; FCL_REAL Rt0_dot_ddRt0 = Rt0.dot(ddRt0); FCL_REAL dRt0_dot_dRt0 = dRt0.sqrLength(); FCL_REAL ddtheta0 = (Rt0_dot_ddRt0 + dRt0_dot_dRt0) * inv_Rt0_len - Rt0_dot_dRt0 * Rt0_dot_dRt0 * inv_Rt0_len_3; Vec3f ddWt0 = ddRt0 * inv_Rt0_len - (dRt0 * (2 * Rt0_dot_dRt0) + Rt0 * (Rt0_dot_ddRt0 + dRt0_dot_dRt0)) * inv_Rt0_len_3 + (Rt0 * (3 * Rt0_dot_dRt0 * Rt0_dot_dRt0)) * inv_Rt0_len_5; Matrix3f hatddWt0; hat(hatddWt0, ddWt0); Matrix3f ddMt0 = hatddWt0 * sintheta0 + hatWt0 * (costheta0 * dtheta0 - sintheta0 * dtheta0 * dtheta0 + costheta0 * ddtheta0) + hatdWt0 * (costheta0 * dtheta0) + (hatWt0 * hatdWt0 + hatdWt0 * hatWt0) * (sintheta0 * dtheta0 * 2) + hatdWt0 * hatdWt0 * (2 * (1 - costheta0)) + hatWt0 * hatWt0 * (costheta0 * dtheta0 * dtheta0 + sintheta0 * ddtheta0) + (hatWt0 * hatddWt0 + hatddWt0 + hatWt0) * (1 - costheta0); tm.setTimeInterval(getTimeInterval()); for(std::size_t i = 0; i < 3; ++i) { for(std::size_t j = 0; j < 3; ++j) { tm(i, j).coeff(0) = Mt0(i, j) - dMt0(i, j) * 0.5 + ddMt0(i, j) * 0.25 * 0.5; tm(i, j).coeff(1) = dMt0(i, j) - ddMt0(i, j) * 0.5; tm(i, j).coeff(2) = ddMt0(i, j) * 0.5; tm(i, j).coeff(3) = 0; tm(i, j).remainder() = Interval(-1/48.0, 1/48.0); /// not correct, should fix } } } protected: void computeSplineParameter() { } FCL_REAL getWeight0(FCL_REAL t) const; FCL_REAL getWeight1(FCL_REAL t) const; FCL_REAL getWeight2(FCL_REAL t) const; FCL_REAL getWeight3(FCL_REAL t) const; Vec3f Td[4]; Vec3f Rd[4]; Vec3f TA, TB, TC; Vec3f RA, RB, RC; FCL_REAL Rd0Rd0, Rd0Rd1, Rd0Rd2, Rd0Rd3, Rd1Rd1, Rd1Rd2, Rd1Rd3, Rd2Rd2, Rd2Rd3, Rd3Rd3; //// @brief The transformation at current time t mutable Transform3f tf; /// @brief The time related with tf mutable FCL_REAL tf_t; public: FCL_REAL computeTBound(const Vec3f& n) const; FCL_REAL computeDWMax() const; FCL_REAL getCurrentTime() const { return tf_t; } }; class ScrewMotion : public MotionBase { public: /// @brief Default transformations are all identities ScrewMotion() : MotionBase() { // Default angular velocity is zero axis.setValue(1, 0, 0); angular_vel = 0; // Default reference point is local zero point // Default linear velocity is zero linear_vel = 0; } /// @brief Construct motion from the initial rotation/translation and goal rotation/translation ScrewMotion(const Matrix3f& R1, const Vec3f& T1, const Matrix3f& R2, const Vec3f& T2) : MotionBase(), tf1(R1, T1), tf2(R2, T2), tf(tf1) { computeScrewParameter(); } /// @brief Construct motion from the initial transform and goal transform ScrewMotion(const Transform3f& tf1_, const Transform3f& tf2_) : tf1(tf1_), tf2(tf2_), tf(tf1) { computeScrewParameter(); } /// @brief Integrate the motion from 0 to dt /// We compute the current transformation from zero point instead of from last integrate time, for precision. bool integrate(double dt) const { if(dt > 1) dt = 1; tf.setQuatRotation(absoluteRotation(dt)); Quaternion3f delta_rot = deltaRotation(dt); tf.setTranslation(p + axis * (dt * linear_vel) + delta_rot.transform(tf1.getTranslation() - p)); return true; } /// @brief Compute the motion bound for a bounding volume along a given direction n, which is defined in the visitor FCL_REAL computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const { return mb_visitor.visit(*this); } /// @brief Compute the motion bound for a triangle along a given direction n, which is defined in the visitor FCL_REAL computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const { return mb_visitor.visit(*this); } /// @brief Get the rotation and translation in current step void getCurrentTransform(Transform3f& tf_) const { tf_ = tf; } void getTaylorModel(TMatrix3& tm, TVector3& tv) const { Matrix3f hat_axis; hat(hat_axis, axis); TaylorModel cos_model(getTimeInterval()); generateTaylorModelForCosFunc(cos_model, angular_vel, 0); TaylorModel sin_model(getTimeInterval()); generateTaylorModelForSinFunc(sin_model, angular_vel, 0); TMatrix3 delta_R = hat_axis * sin_model - hat_axis * hat_axis * (cos_model - 1) + Matrix3f(1, 0, 0, 0, 1, 0, 0, 0, 1); TaylorModel a(getTimeInterval()), b(getTimeInterval()), c(getTimeInterval()); generateTaylorModelForLinearFunc(a, 0, linear_vel * axis[0]); generateTaylorModelForLinearFunc(b, 0, linear_vel * axis[1]); generateTaylorModelForLinearFunc(c, 0, linear_vel * axis[2]); TVector3 delta_T = p - delta_R * p + TVector3(a, b, c); tm = delta_R * tf1.getRotation(); tv = delta_R * tf1.getTranslation() + delta_T; } protected: void computeScrewParameter() { Quaternion3f deltaq = tf2.getQuatRotation() * inverse(tf1.getQuatRotation()); deltaq.toAxisAngle(axis, angular_vel); if(angular_vel < 0) { angular_vel = -angular_vel; axis = -axis; } if(angular_vel < 1e-10) { angular_vel = 0; axis = tf2.getTranslation() - tf1.getTranslation(); linear_vel = axis.length(); p = tf1.getTranslation(); } else { Vec3f o = tf2.getTranslation() - tf1.getTranslation(); p = (tf1.getTranslation() + tf2.getTranslation() + axis.cross(o) * (1.0 / tan(angular_vel / 2.0))) * 0.5; linear_vel = o.dot(axis); } } Quaternion3f deltaRotation(FCL_REAL dt) const { Quaternion3f res; res.fromAxisAngle(axis, (FCL_REAL)(dt * angular_vel)); return res; } Quaternion3f absoluteRotation(FCL_REAL dt) const { Quaternion3f delta_t = deltaRotation(dt); return delta_t * tf1.getQuatRotation(); } /// @brief The transformation at time 0 Transform3f tf1; /// @brief The transformation at time 1 Transform3f tf2; /// @brief The transformation at current time t mutable Transform3f tf; /// @brief screw axis Vec3f axis; /// @brief A point on the axis S Vec3f p; /// @brief linear velocity along the axis FCL_REAL linear_vel; /// @brief angular velocity FCL_REAL angular_vel; public: inline FCL_REAL getLinearVelocity() const { return linear_vel; } inline FCL_REAL getAngularVelocity() const { return angular_vel; } inline const Vec3f& getAxis() const { return axis; } inline const Vec3f& getAxisOrigin() const { return p; } }; /// @brief Linear interpolation motion /// Each Motion is assumed to have constant linear velocity and angular velocity /// The motion is R(t)(p - p_ref) + p_ref + T(t) /// Therefore, R(0) = R0, R(1) = R1 /// T(0) = T0 + R0 p_ref - p_ref /// T(1) = T1 + R1 p_ref - p_ref class InterpMotion : public MotionBase { public: /// @brief Default transformations are all identities InterpMotion(); /// @brief Construct motion from the initial rotation/translation and goal rotation/translation InterpMotion(const Matrix3f& R1, const Vec3f& T1, const Matrix3f& R2, const Vec3f& T2); InterpMotion(const Transform3f& tf1_, const Transform3f& tf2_); /// @brief Construct motion from the initial rotation/translation and goal rotation/translation related to some rotation center InterpMotion(const Matrix3f& R1, const Vec3f& T1, const Matrix3f& R2, const Vec3f& T2, const Vec3f& O); InterpMotion(const Transform3f& tf1_, const Transform3f& tf2_, const Vec3f& O); /// @brief Integrate the motion from 0 to dt /// We compute the current transformation from zero point instead of from last integrate time, for precision. bool integrate(double dt) const; /// @brief Compute the motion bound for a bounding volume along a given direction n, which is defined in the visitor FCL_REAL computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const { return mb_visitor.visit(*this); } /// @brief Compute the motion bound for a triangle along a given direction n, which is defined in the visitor FCL_REAL computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const { return mb_visitor.visit(*this); } /// @brief Get the rotation and translation in current step void getCurrentTransform(Transform3f& tf_) const { tf_ = tf; } void getTaylorModel(TMatrix3& tm, TVector3& tv) const { Matrix3f hat_angular_axis; hat(hat_angular_axis, angular_axis); TaylorModel cos_model(getTimeInterval()); generateTaylorModelForCosFunc(cos_model, angular_vel, 0); TaylorModel sin_model(getTimeInterval()); generateTaylorModelForSinFunc(sin_model, angular_vel, 0); TMatrix3 delta_R = hat_angular_axis * sin_model - hat_angular_axis * hat_angular_axis * (cos_model - 1) + Matrix3f(1, 0, 0, 0, 1, 0, 0, 0, 1); TaylorModel a(getTimeInterval()), b(getTimeInterval()), c(getTimeInterval()); generateTaylorModelForLinearFunc(a, 0, linear_vel[0]); generateTaylorModelForLinearFunc(b, 0, linear_vel[1]); generateTaylorModelForLinearFunc(c, 0, linear_vel[2]); TVector3 delta_T(a, b, c); tm = delta_R * tf1.getRotation(); tv = tf1.transform(reference_p) + delta_T - delta_R * tf1.getQuatRotation().transform(reference_p); } protected: void computeVelocity(); Quaternion3f deltaRotation(FCL_REAL dt) const; Quaternion3f absoluteRotation(FCL_REAL dt) const; /// @brief The transformation at time 0 Transform3f tf1; /// @brief The transformation at time 1 Transform3f tf2; /// @brief The transformation at current time t mutable Transform3f tf; /// @brief Linear velocity Vec3f linear_vel; /// @brief Angular speed FCL_REAL angular_vel; /// @brief Angular velocity axis Vec3f angular_axis; /// @brief Reference point for the motion (in the object's local frame) Vec3f reference_p; public: const Vec3f& getReferencePoint() const { return reference_p; } const Vec3f& getAngularAxis() const { return angular_axis; } FCL_REAL getAngularVelocity() const { return angular_vel; } const Vec3f& getLinearVelocity() const { return linear_vel; } }; } #endif fcl-0.5.0/include/fcl/ccd/motion_base.h000066400000000000000000000133521274356571000176660ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_CCD_MOTION_BASE_H #define FCL_CCD_MOTION_BASE_H #include "fcl/math/transform.h" #include "fcl/ccd/taylor_matrix.h" #include "fcl/ccd/taylor_vector.h" #include "fcl/BV/RSS.h" namespace fcl { class MotionBase; class SplineMotion; class ScrewMotion; class InterpMotion; class TranslationMotion; /// @brief Compute the motion bound for a bounding volume, given the closest direction n between two query objects class BVMotionBoundVisitor { public: virtual FCL_REAL visit(const MotionBase& motion) const = 0; virtual FCL_REAL visit(const SplineMotion& motion) const = 0; virtual FCL_REAL visit(const ScrewMotion& motion) const = 0; virtual FCL_REAL visit(const InterpMotion& motion) const = 0; virtual FCL_REAL visit(const TranslationMotion& motion) const = 0; }; template class TBVMotionBoundVisitor : public BVMotionBoundVisitor { public: TBVMotionBoundVisitor(const BV& bv_, const Vec3f& n_) : bv(bv_), n(n_) {} virtual FCL_REAL visit(const MotionBase& motion) const { return 0; } virtual FCL_REAL visit(const SplineMotion& motion) const { return 0; } virtual FCL_REAL visit(const ScrewMotion& motion) const { return 0; } virtual FCL_REAL visit(const InterpMotion& motion) const { return 0; } virtual FCL_REAL visit(const TranslationMotion& motion) const { return 0; } protected: BV bv; Vec3f n; }; template<> FCL_REAL TBVMotionBoundVisitor::visit(const SplineMotion& motion) const; template<> FCL_REAL TBVMotionBoundVisitor::visit(const ScrewMotion& motion) const; template<> FCL_REAL TBVMotionBoundVisitor::visit(const InterpMotion& motion) const; template<> FCL_REAL TBVMotionBoundVisitor::visit(const TranslationMotion& motion) const; class TriangleMotionBoundVisitor { public: TriangleMotionBoundVisitor(const Vec3f& a_, const Vec3f& b_, const Vec3f& c_, const Vec3f& n_) : a(a_), b(b_), c(c_), n(n_) {} virtual FCL_REAL visit(const MotionBase& motion) const { return 0; } virtual FCL_REAL visit(const SplineMotion& motion) const; virtual FCL_REAL visit(const ScrewMotion& motion) const; virtual FCL_REAL visit(const InterpMotion& motion) const; virtual FCL_REAL visit(const TranslationMotion& motion) const; protected: Vec3f a, b, c, n; }; class MotionBase { public: MotionBase() : time_interval_(std::shared_ptr(new TimeInterval(0, 1))) { } virtual ~MotionBase() {} /** \brief Integrate the motion from 0 to dt */ virtual bool integrate(double dt) const = 0; /** \brief Compute the motion bound for a bounding volume, given the closest direction n between two query objects */ virtual FCL_REAL computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const = 0; /** \brief Compute the motion bound for a triangle, given the closest direction n between two query objects */ virtual FCL_REAL computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const = 0; /** \brief Get the rotation and translation in current step */ void getCurrentTransform(Matrix3f& R, Vec3f& T) const { Transform3f tf; getCurrentTransform(tf); R = tf.getRotation(); T = tf.getTranslation(); } void getCurrentTransform(Quaternion3f& Q, Vec3f& T) const { Transform3f tf; getCurrentTransform(tf); Q = tf.getQuatRotation(); T = tf.getTranslation(); } void getCurrentRotation(Matrix3f& R) const { Transform3f tf; getCurrentTransform(tf); R = tf.getRotation(); } void getCurrentRotation(Quaternion3f& Q) const { Transform3f tf; getCurrentTransform(tf); Q = tf.getQuatRotation(); } void getCurrentTranslation(Vec3f& T) const { Transform3f tf; getCurrentTransform(tf); T = tf.getTranslation(); } virtual void getCurrentTransform(Transform3f& tf) const = 0; virtual void getTaylorModel(TMatrix3& tm, TVector3& tv) const = 0; const std::shared_ptr& getTimeInterval() const { return time_interval_; } protected: std::shared_ptr time_interval_; }; typedef std::shared_ptr MotionBasePtr; } #endif fcl-0.5.0/include/fcl/ccd/simplex.h000066400000000000000000000052771274356571000170570ustar00rootroot00000000000000/*** * libccd * --------------------------------- * Copyright (c)2010 Daniel Fiser * * * This file is part of libccd. * * Distributed under the OSI-approved BSD License (the "License"); * see accompanying file BDS-LICENSE for details or see * . * * This software is distributed WITHOUT ANY WARRANTY; without even the * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. * See the License for more information. */ #ifndef __CCD_SIMPLEX_H__ #define __CCD_SIMPLEX_H__ #include #include "support.h" #ifdef __cplusplus extern "C" { #endif /* __cplusplus */ struct _ccd_simplex_t { ccd_support_t ps[4]; int last; //!< index of last added point }; typedef struct _ccd_simplex_t ccd_simplex_t; _ccd_inline void ccdSimplexInit(ccd_simplex_t *s); _ccd_inline int ccdSimplexSize(const ccd_simplex_t *s); _ccd_inline const ccd_support_t *ccdSimplexLast(const ccd_simplex_t *s); _ccd_inline const ccd_support_t *ccdSimplexPoint(const ccd_simplex_t *s, int idx); _ccd_inline ccd_support_t *ccdSimplexPointW(ccd_simplex_t *s, int idx); _ccd_inline void ccdSimplexAdd(ccd_simplex_t *s, const ccd_support_t *v); _ccd_inline void ccdSimplexSet(ccd_simplex_t *s, size_t pos, const ccd_support_t *a); _ccd_inline void ccdSimplexSetSize(ccd_simplex_t *s, int size); _ccd_inline void ccdSimplexSwap(ccd_simplex_t *s, size_t pos1, size_t pos2); /**** INLINES ****/ _ccd_inline void ccdSimplexInit(ccd_simplex_t *s) { s->last = -1; } _ccd_inline int ccdSimplexSize(const ccd_simplex_t *s) { return s->last + 1; } _ccd_inline const ccd_support_t *ccdSimplexLast(const ccd_simplex_t *s) { return ccdSimplexPoint(s, s->last); } _ccd_inline const ccd_support_t *ccdSimplexPoint(const ccd_simplex_t *s, int idx) { // here is no check on boundaries return &s->ps[idx]; } _ccd_inline ccd_support_t *ccdSimplexPointW(ccd_simplex_t *s, int idx) { return &s->ps[idx]; } _ccd_inline void ccdSimplexAdd(ccd_simplex_t *s, const ccd_support_t *v) { // here is no check on boundaries in sake of speed ++s->last; ccdSupportCopy(s->ps + s->last, v); } _ccd_inline void ccdSimplexSet(ccd_simplex_t *s, size_t pos, const ccd_support_t *a) { ccdSupportCopy(s->ps + pos, a); } _ccd_inline void ccdSimplexSetSize(ccd_simplex_t *s, int size) { s->last = size - 1; } _ccd_inline void ccdSimplexSwap(ccd_simplex_t *s, size_t pos1, size_t pos2) { ccd_support_t supp; ccdSupportCopy(&supp, &s->ps[pos1]); ccdSupportCopy(&s->ps[pos1], &s->ps[pos2]); ccdSupportCopy(&s->ps[pos2], &supp); } #ifdef __cplusplus } /* extern "C" */ #endif /* __cplusplus */ #endif /* __CCD_SIMPLEX_H__ */ fcl-0.5.0/include/fcl/ccd/support.h000066400000000000000000000026231274356571000171020ustar00rootroot00000000000000/*** * libccd * --------------------------------- * Copyright (c)2010 Daniel Fiser * * * This file is part of libccd. * * Distributed under the OSI-approved BSD License (the "License"); * see accompanying file BDS-LICENSE for details or see * . * * This software is distributed WITHOUT ANY WARRANTY; without even the * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. * See the License for more information. */ #ifndef __CCD_SUPPORT_H__ #define __CCD_SUPPORT_H__ #include #ifdef __cplusplus extern "C" { #endif /* __cplusplus */ struct _ccd_support_t { ccd_vec3_t v; //!< Support point in minkowski sum ccd_vec3_t v1; //!< Support point in obj1 ccd_vec3_t v2; //!< Support point in obj2 }; typedef struct _ccd_support_t ccd_support_t; _ccd_inline void ccdSupportCopy(ccd_support_t *, const ccd_support_t *s); /** * Computes support point of obj1 and obj2 in direction dir. * Support point is returned via supp. */ void __ccdSupport(const void *obj1, const void *obj2, const ccd_vec3_t *dir, const ccd_t *ccd, ccd_support_t *supp); /**** INLINES ****/ _ccd_inline void ccdSupportCopy(ccd_support_t *d, const ccd_support_t *s) { *d = *s; } #ifdef __cplusplus } /* extern "C" */ #endif /* __cplusplus */ #endif /* __CCD_SUPPORT_H__ */ fcl-0.5.0/include/fcl/ccd/taylor_matrix.h000066400000000000000000000105121274356571000202600ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ // This code is based on code developed by Stephane Redon at UNC and Inria for the CATCH library: http://graphics.ewha.ac.kr/CATCH/ /** \author Jia Pan */ #ifndef FCL_CCD_TAYLOR_MATRIX_H #define FCL_CCD_TAYLOR_MATRIX_H #include "fcl/math/matrix_3f.h" #include "fcl/ccd/taylor_vector.h" #include "fcl/ccd/interval_matrix.h" namespace fcl { class TMatrix3 { TVector3 v_[3]; public: TMatrix3(); TMatrix3(const std::shared_ptr& time_interval); TMatrix3(TaylorModel m[3][3]); TMatrix3(const TVector3& v1, const TVector3& v2, const TVector3& v3); TMatrix3(const Matrix3f& m, const std::shared_ptr& time_interval); TVector3 getColumn(size_t i) const; const TVector3& getRow(size_t i) const; const TaylorModel& operator () (size_t i, size_t j) const; TaylorModel& operator () (size_t i, size_t j); TVector3 operator * (const Vec3f& v) const; TVector3 operator * (const TVector3& v) const; TMatrix3 operator * (const Matrix3f& m) const; TMatrix3 operator * (const TMatrix3& m) const; TMatrix3 operator * (const TaylorModel& d) const; TMatrix3 operator * (FCL_REAL d) const; TMatrix3& operator *= (const Matrix3f& m); TMatrix3& operator *= (const TMatrix3& m); TMatrix3& operator *= (const TaylorModel& d); TMatrix3& operator *= (FCL_REAL d); TMatrix3 operator + (const TMatrix3& m) const; TMatrix3& operator += (const TMatrix3& m); TMatrix3 operator + (const Matrix3f& m) const; TMatrix3& operator += (const Matrix3f& m); TMatrix3 operator - (const TMatrix3& m) const; TMatrix3& operator -= (const TMatrix3& m); TMatrix3 operator - (const Matrix3f& m) const; TMatrix3& operator -= (const Matrix3f& m); TMatrix3 operator - () const; IMatrix3 getBound() const; IMatrix3 getBound(FCL_REAL l, FCL_REAL r) const; IMatrix3 getBound(FCL_REAL t) const; IMatrix3 getTightBound() const; IMatrix3 getTightBound(FCL_REAL l, FCL_REAL r) const; void print() const; void setIdentity(); void setZero(); FCL_REAL diameter() const; void setTimeInterval(const std::shared_ptr& time_interval); void setTimeInterval(FCL_REAL l, FCL_REAL r); const std::shared_ptr& getTimeInterval() const; TMatrix3& rotationConstrain(); }; TMatrix3 rotationConstrain(const TMatrix3& m); TMatrix3 operator * (const Matrix3f& m, const TaylorModel& a); TMatrix3 operator * (const TaylorModel& a, const Matrix3f& m); TMatrix3 operator * (const TaylorModel& a, const TMatrix3& m); TMatrix3 operator * (FCL_REAL d, const TMatrix3& m); TMatrix3 operator + (const Matrix3f& m1, const TMatrix3& m2); TMatrix3 operator - (const Matrix3f& m1, const TMatrix3& m2); } #endif fcl-0.5.0/include/fcl/ccd/taylor_model.h000066400000000000000000000126531274356571000200640ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ // This code is based on code developed by Stephane Redon at UNC and Inria for the CATCH library: http://graphics.ewha.ac.kr/CATCH/ /** \author Jia Pan */ #ifndef FCL_CCD_TAYLOR_MODEL_H #define FCL_CCD_TAYLOR_MODEL_H #include "fcl/ccd/interval.h" #include namespace fcl { struct TimeInterval { /// @brief time interval and different powers Interval t_; // [t1, t2] Interval t2_; // [t1, t2]^2 Interval t3_; // [t1, t2]^3 Interval t4_; // [t1, t2]^4 Interval t5_; // [t1, t2]^5 Interval t6_; // [t1, t2]^6 TimeInterval() {} TimeInterval(FCL_REAL l, FCL_REAL r) { setValue(l, r); } void setValue(FCL_REAL l, FCL_REAL r) { t_.setValue(l, r); t2_.setValue(l * t_[0], r * t_[1]); t3_.setValue(l * t2_[0], r * t2_[1]); t4_.setValue(l * t3_[0], r * t3_[1]); t5_.setValue(l * t4_[0], r * t4_[1]); t6_.setValue(l * t5_[0], r * t5_[1]); } }; /// @brief TaylorModel implements a third order Taylor model, i.e., a cubic approximation of a function /// over a time interval, with an interval remainder. /// All the operations on two Taylor models assume their time intervals are the same. class TaylorModel { /// @brief time interval std::shared_ptr time_interval_; /// @brief Coefficients of the cubic polynomial approximation FCL_REAL coeffs_[4]; /// @brief interval remainder Interval r_; public: void setTimeInterval(FCL_REAL l, FCL_REAL r) { time_interval_->setValue(l, r); } void setTimeInterval(const std::shared_ptr& time_interval) { time_interval_ = time_interval; } const std::shared_ptr& getTimeInterval() const { return time_interval_; } FCL_REAL coeff(std::size_t i) const { return coeffs_[i]; } FCL_REAL& coeff(std::size_t i) { return coeffs_[i]; } const Interval& remainder() const { return r_; } Interval& remainder() { return r_; } TaylorModel(); TaylorModel(const std::shared_ptr& time_interval); TaylorModel(FCL_REAL coeff, const std::shared_ptr& time_interval); TaylorModel(FCL_REAL coeffs[3], const Interval& r, const std::shared_ptr& time_interval); TaylorModel(FCL_REAL c0, FCL_REAL c1, FCL_REAL c2, FCL_REAL c3, const Interval& r, const std::shared_ptr& time_interval); TaylorModel operator + (const TaylorModel& other) const; TaylorModel& operator += (const TaylorModel& other); TaylorModel operator - (const TaylorModel& other) const; TaylorModel& operator -= (const TaylorModel& other); TaylorModel operator + (FCL_REAL d) const; TaylorModel& operator += (FCL_REAL d); TaylorModel operator - (FCL_REAL d) const; TaylorModel& operator -= (FCL_REAL d); TaylorModel operator * (const TaylorModel& other) const; TaylorModel operator * (FCL_REAL d) const; TaylorModel& operator *= (const TaylorModel& other); TaylorModel& operator *= (FCL_REAL d); TaylorModel operator - () const; void print() const; Interval getBound() const; Interval getBound(FCL_REAL l, FCL_REAL r) const; Interval getTightBound() const; Interval getTightBound(FCL_REAL l, FCL_REAL r) const; Interval getBound(FCL_REAL t) const; void setZero(); }; TaylorModel operator * (FCL_REAL d, const TaylorModel& a); TaylorModel operator + (FCL_REAL d, const TaylorModel& a); TaylorModel operator - (FCL_REAL d, const TaylorModel& a); /// @brief Generate Taylor model for cos(w t + q0) void generateTaylorModelForCosFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0); /// @brief Generate Taylor model for sin(w t + q0) void generateTaylorModelForSinFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0); /// @brief Generate Taylor model for p + v t void generateTaylorModelForLinearFunc(TaylorModel& tm, FCL_REAL p, FCL_REAL v); } #endif fcl-0.5.0/include/fcl/ccd/taylor_vector.h000066400000000000000000000077371274356571000202750ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ // This code is based on code developed by Stephane Redon at UNC and Inria for the CATCH library: http://graphics.ewha.ac.kr/CATCH/ /** \author Jia Pan */ #ifndef FCL_CCD_TAYLOR_VECTOR_H #define FCL_CCD_TAYLOR_VECTOR_H #include "fcl/ccd/interval_vector.h" #include "fcl/ccd/taylor_model.h" namespace fcl { class TVector3 { TaylorModel i_[3]; public: TVector3(); TVector3(const std::shared_ptr& time_interval); TVector3(TaylorModel v[3]); TVector3(const TaylorModel& v0, const TaylorModel& v1, const TaylorModel& v2); TVector3(const Vec3f& v, const std::shared_ptr& time_interval); TVector3 operator + (const TVector3& other) const; TVector3& operator += (const TVector3& other); TVector3 operator + (const Vec3f& other) const; TVector3& operator += (const Vec3f& other); TVector3 operator - (const TVector3& other) const; TVector3& operator -= (const TVector3& other); TVector3 operator - (const Vec3f& other) const; TVector3& operator -= (const Vec3f& other); TVector3 operator - () const; TVector3 operator * (const TaylorModel& d) const; TVector3& operator *= (const TaylorModel& d); TVector3 operator * (FCL_REAL d) const; TVector3& operator *= (FCL_REAL d); const TaylorModel& operator [] (size_t i) const; TaylorModel& operator [] (size_t i); TaylorModel dot(const TVector3& other) const; TVector3 cross(const TVector3& other) const; TaylorModel dot(const Vec3f& other) const; TVector3 cross(const Vec3f& other) const; IVector3 getBound() const; IVector3 getBound(FCL_REAL l, FCL_REAL r) const; IVector3 getBound(FCL_REAL t) const; IVector3 getTightBound() const; IVector3 getTightBound(FCL_REAL l, FCL_REAL r) const; void print() const; FCL_REAL volumn() const; void setZero(); TaylorModel squareLength() const; void setTimeInterval(const std::shared_ptr& time_interval); void setTimeInterval(FCL_REAL l, FCL_REAL r); const std::shared_ptr& getTimeInterval() const; }; void generateTVector3ForLinearFunc(TVector3& v, const Vec3f& position, const Vec3f& velocity); TVector3 operator * (const Vec3f& v, const TaylorModel& a); TVector3 operator + (const Vec3f& v1, const TVector3& v2); TVector3 operator - (const Vec3f& v1, const TVector3& v2); } #endif fcl-0.5.0/include/fcl/collision.h000066400000000000000000000054471274356571000166370ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_COLLISION_H #define FCL_COLLISION_H #include "fcl/math/vec_3f.h" #include "fcl/collision_object.h" #include "fcl/collision_data.h" namespace fcl { /// @brief Main collision interface: given two collision objects, and the requirements for contacts, including num of max contacts, whether perform exhaustive collision (i.e., returning /// returning all the contact points), whether return detailed contact information (i.e., normal, contact point, depth; otherwise only contact primitive id is returned), this function /// performs the collision between them. /// Return value is the number of contacts generated between the two objects. std::size_t collide(const CollisionObject* o1, const CollisionObject* o2, const CollisionRequest& request, CollisionResult& result); std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result); } #endif fcl-0.5.0/include/fcl/collision_data.h000066400000000000000000000406251274356571000176250ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_COLLISION_DATA_H #define FCL_COLLISION_DATA_H #include "fcl/collision_object.h" #include "fcl/learning/classifier.h" #include "fcl/math/vec_3f.h" #include #include #include #include namespace fcl { /// @brief Type of narrow phase GJK solver enum GJKSolverType {GST_LIBCCD, GST_INDEP}; /// @brief Minimal contact information returned by collision struct ContactPoint { /// @brief Contact normal, pointing from o1 to o2 Vec3f normal; /// @brief Contact position, in world space Vec3f pos; /// @brief Penetration depth FCL_REAL penetration_depth; /// @brief Constructor ContactPoint() : normal(Vec3f()), pos(Vec3f()), penetration_depth(0.0) {} /// @brief Constructor ContactPoint(const Vec3f& n_, const Vec3f& p_, FCL_REAL d_) : normal(n_), pos(p_), penetration_depth(d_) {} }; /// @brief Return true if _cp1's penentration depth is less than _cp2's. bool comparePenDepth(const ContactPoint& _cp1, const ContactPoint& _cp2); /// @brief Contact information returned by collision struct Contact { /// @brief collision object 1 const CollisionGeometry* o1; /// @brief collision object 2 const CollisionGeometry* o2; /// @brief contact primitive in object 1 /// if object 1 is mesh or point cloud, it is the triangle or point id /// if object 1 is geometry shape, it is NONE (-1), /// if object 1 is octree, it is the id of the cell int b1; /// @brief contact primitive in object 2 /// if object 2 is mesh or point cloud, it is the triangle or point id /// if object 2 is geometry shape, it is NONE (-1), /// if object 2 is octree, it is the id of the cell int b2; /// @brief contact normal, pointing from o1 to o2 Vec3f normal; /// @brief contact position, in world space Vec3f pos; /// @brief penetration depth FCL_REAL penetration_depth; /// @brief invalid contact primitive information static const int NONE = -1; Contact() : o1(NULL), o2(NULL), b1(NONE), b2(NONE) {} Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_) : o1(o1_), o2(o2_), b1(b1_), b2(b2_) {} Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_, const Vec3f& pos_, const Vec3f& normal_, FCL_REAL depth_) : o1(o1_), o2(o2_), b1(b1_), b2(b2_), normal(normal_), pos(pos_), penetration_depth(depth_) {} bool operator < (const Contact& other) const { if(b1 == other.b1) return b2 < other.b2; return b1 < other.b1; } }; /// @brief Cost source describes an area with a cost. The area is described by an AABB region. struct CostSource { /// @brief aabb lower bound Vec3f aabb_min; /// @brief aabb upper bound Vec3f aabb_max; /// @brief cost density in the AABB region FCL_REAL cost_density; FCL_REAL total_cost; CostSource(const Vec3f& aabb_min_, const Vec3f& aabb_max_, FCL_REAL cost_density_) : aabb_min(aabb_min_), aabb_max(aabb_max_), cost_density(cost_density_) { total_cost = cost_density * (aabb_max[0] - aabb_min[0]) * (aabb_max[1] - aabb_min[1]) * (aabb_max[2] - aabb_min[2]); } CostSource(const AABB& aabb, FCL_REAL cost_density_) : aabb_min(aabb.min_), aabb_max(aabb.max_), cost_density(cost_density_) { total_cost = cost_density * (aabb_max[0] - aabb_min[0]) * (aabb_max[1] - aabb_min[1]) * (aabb_max[2] - aabb_min[2]); } CostSource() {} bool operator < (const CostSource& other) const { if(total_cost < other.total_cost) return false; if(total_cost > other.total_cost) return true; if(cost_density < other.cost_density) return false; if(cost_density > other.cost_density) return true; for(size_t i = 0; i < 3; ++i) if(aabb_min[i] != other.aabb_min[i]) return aabb_min[i] < other.aabb_min[i]; return false; } }; struct CollisionResult; /// @brief request to the collision algorithm struct CollisionRequest { /// @brief The maximum number of contacts will return size_t num_max_contacts; /// @brief whether the contact information (normal, penetration depth and contact position) will return bool enable_contact; /// @brief The maximum number of cost sources will return size_t num_max_cost_sources; /// @brief whether the cost sources will be computed bool enable_cost; /// @brief whether the cost computation is approximated bool use_approximate_cost; /// @brief narrow phase solver GJKSolverType gjk_solver_type; /// @brief whether enable gjk intial guess bool enable_cached_gjk_guess; /// @brief the gjk intial guess set by user Vec3f cached_gjk_guess; CollisionRequest(size_t num_max_contacts_ = 1, bool enable_contact_ = false, size_t num_max_cost_sources_ = 1, bool enable_cost_ = false, bool use_approximate_cost_ = true, GJKSolverType gjk_solver_type_ = GST_LIBCCD) : num_max_contacts(num_max_contacts_), enable_contact(enable_contact_), num_max_cost_sources(num_max_cost_sources_), enable_cost(enable_cost_), use_approximate_cost(use_approximate_cost_), gjk_solver_type(gjk_solver_type_) { enable_cached_gjk_guess = false; cached_gjk_guess = Vec3f(1, 0, 0); } bool isSatisfied(const CollisionResult& result) const; }; /// @brief collision result struct CollisionResult { private: /// @brief contact information std::vector contacts; /// @brief cost sources std::set cost_sources; public: Vec3f cached_gjk_guess; public: CollisionResult() { } /// @brief add one contact into result structure inline void addContact(const Contact& c) { contacts.push_back(c); } /// @brief add one cost source into result structure inline void addCostSource(const CostSource& c, std::size_t num_max_cost_sources) { cost_sources.insert(c); while (cost_sources.size() > num_max_cost_sources) cost_sources.erase(--cost_sources.end()); } /// @brief return binary collision result bool isCollision() const { return contacts.size() > 0; } /// @brief number of contacts found size_t numContacts() const { return contacts.size(); } /// @brief number of cost sources found size_t numCostSources() const { return cost_sources.size(); } /// @brief get the i-th contact calculated const Contact& getContact(size_t i) const { if(i < contacts.size()) return contacts[i]; else return contacts.back(); } /// @brief get all the contacts void getContacts(std::vector& contacts_) { contacts_.resize(contacts.size()); std::copy(contacts.begin(), contacts.end(), contacts_.begin()); } /// @brief get all the cost sources void getCostSources(std::vector& cost_sources_) { cost_sources_.resize(cost_sources.size()); std::copy(cost_sources.begin(), cost_sources.end(), cost_sources_.begin()); } /// @brief clear the results obtained void clear() { contacts.clear(); cost_sources.clear(); } }; struct DistanceResult; /// @brief request to the distance computation struct DistanceRequest { /// @brief whether to return the nearest points bool enable_nearest_points; /// @brief error threshold for approximate distance FCL_REAL rel_err; // relative error, between 0 and 1 FCL_REAL abs_err; // absoluate error /// @brief narrow phase solver type GJKSolverType gjk_solver_type; DistanceRequest(bool enable_nearest_points_ = false, FCL_REAL rel_err_ = 0.0, FCL_REAL abs_err_ = 0.0, GJKSolverType gjk_solver_type_ = GST_LIBCCD) : enable_nearest_points(enable_nearest_points_), rel_err(rel_err_), abs_err(abs_err_), gjk_solver_type(gjk_solver_type_) { } bool isSatisfied(const DistanceResult& result) const; }; /// @brief distance result struct DistanceResult { public: /// @brief minimum distance between two objects. if two objects are in collision, min_distance <= 0. FCL_REAL min_distance; /// @brief nearest points Vec3f nearest_points[2]; /// @brief collision object 1 const CollisionGeometry* o1; /// @brief collision object 2 const CollisionGeometry* o2; /// @brief information about the nearest point in object 1 /// if object 1 is mesh or point cloud, it is the triangle or point id /// if object 1 is geometry shape, it is NONE (-1), /// if object 1 is octree, it is the id of the cell int b1; /// @brief information about the nearest point in object 2 /// if object 2 is mesh or point cloud, it is the triangle or point id /// if object 2 is geometry shape, it is NONE (-1), /// if object 2 is octree, it is the id of the cell int b2; /// @brief invalid contact primitive information static const int NONE = -1; DistanceResult(FCL_REAL min_distance_ = std::numeric_limits::max()) : min_distance(min_distance_), o1(NULL), o2(NULL), b1(NONE), b2(NONE) { } /// @brief add distance information into the result void update(FCL_REAL distance, const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_) { if(min_distance > distance) { min_distance = distance; o1 = o1_; o2 = o2_; b1 = b1_; b2 = b2_; } } /// @brief add distance information into the result void update(FCL_REAL distance, const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_, const Vec3f& p1, const Vec3f& p2) { if(min_distance > distance) { min_distance = distance; o1 = o1_; o2 = o2_; b1 = b1_; b2 = b2_; nearest_points[0] = p1; nearest_points[1] = p2; } } /// @brief add distance information into the result void update(const DistanceResult& other_result) { if(min_distance > other_result.min_distance) { min_distance = other_result.min_distance; o1 = other_result.o1; o2 = other_result.o2; b1 = other_result.b1; b2 = other_result.b2; nearest_points[0] = other_result.nearest_points[0]; nearest_points[1] = other_result.nearest_points[1]; } } /// @brief clear the result void clear() { min_distance = std::numeric_limits::max(); o1 = NULL; o2 = NULL; b1 = NONE; b2 = NONE; } }; enum CCDMotionType {CCDM_TRANS, CCDM_LINEAR, CCDM_SCREW, CCDM_SPLINE}; enum CCDSolverType {CCDC_NAIVE, CCDC_CONSERVATIVE_ADVANCEMENT, CCDC_RAY_SHOOTING, CCDC_POLYNOMIAL_SOLVER}; struct ContinuousCollisionRequest { /// @brief maximum num of iterations std::size_t num_max_iterations; /// @brief error in first contact time FCL_REAL toc_err; /// @brief ccd motion type CCDMotionType ccd_motion_type; /// @brief gjk solver type GJKSolverType gjk_solver_type; /// @brief ccd solver type CCDSolverType ccd_solver_type; ContinuousCollisionRequest(std::size_t num_max_iterations_ = 10, FCL_REAL toc_err_ = 0.0001, CCDMotionType ccd_motion_type_ = CCDM_TRANS, GJKSolverType gjk_solver_type_ = GST_LIBCCD, CCDSolverType ccd_solver_type_ = CCDC_NAIVE) : num_max_iterations(num_max_iterations_), toc_err(toc_err_), ccd_motion_type(ccd_motion_type_), gjk_solver_type(gjk_solver_type_), ccd_solver_type(ccd_solver_type_) { } }; /// @brief continuous collision result struct ContinuousCollisionResult { /// @brief collision or not bool is_collide; /// @brief time of contact in [0, 1] FCL_REAL time_of_contact; Transform3f contact_tf1, contact_tf2; ContinuousCollisionResult() : is_collide(false), time_of_contact(1.0) { } }; enum PenetrationDepthType {PDT_TRANSLATIONAL, PDT_GENERAL_EULER, PDT_GENERAL_QUAT, PDT_GENERAL_EULER_BALL, PDT_GENERAL_QUAT_BALL}; struct PenetrationDepthRequest { void* classifier; /// @brief PD algorithm type PenetrationDepthType pd_type; /// @brief gjk solver type GJKSolverType gjk_solver_type; std::vector contact_vectors; PenetrationDepthRequest(void* classifier_, PenetrationDepthType pd_type_ = PDT_TRANSLATIONAL, GJKSolverType gjk_solver_type_ = GST_LIBCCD) : classifier(classifier_), pd_type(pd_type_), gjk_solver_type(gjk_solver_type_) { } }; struct PenetrationDepthResult { /// @brief penetration depth value FCL_REAL pd_value; /// @brief the transform where the collision is resolved Transform3f resolved_tf; }; } #endif fcl-0.5.0/include/fcl/collision_func_matrix.h000066400000000000000000000057401274356571000212320ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_COLLISION_FUNC_MATRIX_H #define FCL_COLLISION_FUNC_MATRIX_H #include "fcl/collision_object.h" #include "fcl/collision_data.h" namespace fcl { /// @brief collision matrix stores the functions for collision between different types of objects and provides a uniform call interface template struct CollisionFunctionMatrix { /// @brief the uniform call interface for collision: for collision, we need know /// 1. two objects o1 and o2 and their configuration in world coordinate tf1 and tf2; /// 2. the solver for narrow phase collision, this is for the collision between geometric shapes; /// 3. the request setting for collision (e.g., whether need to return normal information, whether need to compute cost); /// 4. the structure to return collision result typedef std::size_t (*CollisionFunc)(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result); /// @brief each item in the collision matrix is a function to handle collision between objects of type1 and type2 CollisionFunc collision_matrix[NODE_COUNT][NODE_COUNT]; CollisionFunctionMatrix(); }; } #endif fcl-0.5.0/include/fcl/collision_node.h000066400000000000000000000056221274356571000176370ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_COLLISION_NODE_H #define FCL_COLLISION_NODE_H #include "fcl/traversal/traversal_node_base.h" #include "fcl/traversal/traversal_node_bvhs.h" #include "fcl/BVH/BVH_front.h" /// @brief collision and distance function on traversal nodes. these functions provide a higher level abstraction for collision functions provided in collision_func_matrix namespace fcl { /// @brief collision on collision traversal node; can use front list to accelerate void collide(CollisionTraversalNodeBase* node, BVHFrontList* front_list = NULL); /// @brief self collision on collision traversal node; can use front list to accelerate void selfCollide(CollisionTraversalNodeBase* node, BVHFrontList* front_list = NULL); /// @brief distance computation on distance traversal node; can use front list to accelerate void distance(DistanceTraversalNodeBase* node, BVHFrontList* front_list = NULL, int qsize = 2); /// @brief special collision on OBB traversal node void collide2(MeshCollisionTraversalNodeOBB* node, BVHFrontList* front_list = NULL); /// @brief special collision on RSS traversal node void collide2(MeshCollisionTraversalNodeRSS* node, BVHFrontList* front_list = NULL); } #endif fcl-0.5.0/include/fcl/collision_object.h000066400000000000000000000306601274356571000201600ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_COLLISION_OBJECT_BASE_H #define FCL_COLLISION_OBJECT_BASE_H #include #include "fcl/BV/AABB.h" #include "fcl/math/transform.h" #include "fcl/ccd/motion_base.h" #include namespace fcl { /// @brief object type: BVH (mesh, points), basic geometry, octree enum OBJECT_TYPE {OT_UNKNOWN, OT_BVH, OT_GEOM, OT_OCTREE, OT_COUNT}; /// @brief traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18, kDOP24), basic shape (box, sphere, ellipsoid, capsule, cone, cylinder, convex, plane, halfspace, triangle), and octree enum NODE_TYPE {BV_UNKNOWN, BV_AABB, BV_OBB, BV_RSS, BV_kIOS, BV_OBBRSS, BV_KDOP16, BV_KDOP18, BV_KDOP24, GEOM_BOX, GEOM_SPHERE, GEOM_ELLIPSOID, GEOM_CAPSULE, GEOM_CONE, GEOM_CYLINDER, GEOM_CONVEX, GEOM_PLANE, GEOM_HALFSPACE, GEOM_TRIANGLE, GEOM_OCTREE, NODE_COUNT}; /// @brief The geometry for the object for collision or distance computation class CollisionGeometry { public: CollisionGeometry() : cost_density(1), threshold_occupied(1), threshold_free(0) { } virtual ~CollisionGeometry() {} /// @brief get the type of the object virtual OBJECT_TYPE getObjectType() const { return OT_UNKNOWN; } /// @brief get the node type virtual NODE_TYPE getNodeType() const { return BV_UNKNOWN; } /// @brief compute the AABB for object in local coordinate virtual void computeLocalAABB() = 0; /// @brief get user data in geometry void* getUserData() const { return user_data; } /// @brief set user data in geometry void setUserData(void *data) { user_data = data; } /// @brief whether the object is completely occupied inline bool isOccupied() const { return cost_density >= threshold_occupied; } /// @brief whether the object is completely free inline bool isFree() const { return cost_density <= threshold_free; } /// @brief whether the object has some uncertainty inline bool isUncertain() const { return !isOccupied() && !isFree(); } /// @brief AABB center in local coordinate Vec3f aabb_center; /// @brief AABB radius FCL_REAL aabb_radius; /// @brief AABB in local coordinate, used for tight AABB when only translation transform AABB aabb_local; /// @brief pointer to user defined data specific to this object void *user_data; /// @brief collision cost for unit volume FCL_REAL cost_density; /// @brief threshold for occupied ( >= is occupied) FCL_REAL threshold_occupied; /// @brief threshold for free (<= is free) FCL_REAL threshold_free; /// @brief compute center of mass virtual Vec3f computeCOM() const { return Vec3f(); } /// @brief compute the inertia matrix, related to the origin virtual Matrix3f computeMomentofInertia() const { return Matrix3f(); } /// @brief compute the volume virtual FCL_REAL computeVolume() const { return 0; } /// @brief compute the inertia matrix, related to the com virtual Matrix3f computeMomentofInertiaRelatedToCOM() const { Matrix3f C = computeMomentofInertia(); Vec3f com = computeCOM(); FCL_REAL V = computeVolume(); return Matrix3f(C(0, 0) - V * (com[1] * com[1] + com[2] * com[2]), C(0, 1) + V * com[0] * com[1], C(0, 2) + V * com[0] * com[2], C(1, 0) + V * com[1] * com[0], C(1, 1) - V * (com[0] * com[0] + com[2] * com[2]), C(1, 2) + V * com[1] * com[2], C(2, 0) + V * com[2] * com[0], C(2, 1) + V * com[2] * com[1], C(2, 2) - V * (com[0] * com[0] + com[1] * com[1])); } }; /// @brief the object for collision or distance computation, contains the geometry and the transform information class CollisionObject { public: CollisionObject(const std::shared_ptr &cgeom_) : cgeom(cgeom_), cgeom_const(cgeom_) { if (cgeom) { cgeom->computeLocalAABB(); computeAABB(); } } CollisionObject(const std::shared_ptr &cgeom_, const Transform3f& tf) : cgeom(cgeom_), cgeom_const(cgeom_), t(tf) { cgeom->computeLocalAABB(); computeAABB(); } CollisionObject(const std::shared_ptr &cgeom_, const Matrix3f& R, const Vec3f& T): cgeom(cgeom_), cgeom_const(cgeom_), t(Transform3f(R, T)) { cgeom->computeLocalAABB(); computeAABB(); } ~CollisionObject() { } /// @brief get the type of the object OBJECT_TYPE getObjectType() const { return cgeom->getObjectType(); } /// @brief get the node type NODE_TYPE getNodeType() const { return cgeom->getNodeType(); } /// @brief get the AABB in world space inline const AABB& getAABB() const { return aabb; } /// @brief compute the AABB in world space inline void computeAABB() { if(t.getQuatRotation().isIdentity()) { aabb = translate(cgeom->aabb_local, t.getTranslation()); } else { Vec3f center = t.transform(cgeom->aabb_center); Vec3f delta(cgeom->aabb_radius); aabb.min_ = center - delta; aabb.max_ = center + delta; } } /// @brief get user data in object void* getUserData() const { return user_data; } /// @brief set user data in object void setUserData(void *data) { user_data = data; } /// @brief get translation of the object inline const Vec3f& getTranslation() const { return t.getTranslation(); } /// @brief get matrix rotation of the object inline const Matrix3f& getRotation() const { return t.getRotation(); } /// @brief get quaternion rotation of the object inline const Quaternion3f& getQuatRotation() const { return t.getQuatRotation(); } /// @brief get object's transform inline const Transform3f& getTransform() const { return t; } /// @brief set object's rotation matrix void setRotation(const Matrix3f& R) { t.setRotation(R); } /// @brief set object's translation void setTranslation(const Vec3f& T) { t.setTranslation(T); } /// @brief set object's quatenrion rotation void setQuatRotation(const Quaternion3f& q) { t.setQuatRotation(q); } /// @brief set object's transform void setTransform(const Matrix3f& R, const Vec3f& T) { t.setTransform(R, T); } /// @brief set object's transform void setTransform(const Quaternion3f& q, const Vec3f& T) { t.setTransform(q, T); } /// @brief set object's transform void setTransform(const Transform3f& tf) { t = tf; } /// @brief whether the object is in local coordinate bool isIdentityTransform() const { return t.isIdentity(); } /// @brief set the object in local coordinate void setIdentityTransform() { t.setIdentity(); } /// @brief get geometry from the object instance FCL_DEPRECATED const CollisionGeometry* getCollisionGeometry() const { return cgeom.get(); } /// @brief get geometry from the object instance const std::shared_ptr& collisionGeometry() const { return cgeom_const; } /// @brief get object's cost density FCL_REAL getCostDensity() const { return cgeom->cost_density; } /// @brief set object's cost density void setCostDensity(FCL_REAL c) { cgeom->cost_density = c; } /// @brief whether the object is completely occupied inline bool isOccupied() const { return cgeom->isOccupied(); } /// @brief whether the object is completely free inline bool isFree() const { return cgeom->isFree(); } /// @brief whether the object is uncertain inline bool isUncertain() const { return cgeom->isUncertain(); } protected: std::shared_ptr cgeom; std::shared_ptr cgeom_const; Transform3f t; /// @brief AABB in global coordinate mutable AABB aabb; /// @brief pointer to user defined data specific to this object void *user_data; }; /// @brief the object for continuous collision or distance computation, contains the geometry and the motion information class ContinuousCollisionObject { public: ContinuousCollisionObject(const std::shared_ptr& cgeom_) : cgeom(cgeom_), cgeom_const(cgeom_) { } ContinuousCollisionObject(const std::shared_ptr& cgeom_, const std::shared_ptr& motion_) : cgeom(cgeom_), cgeom_const(cgeom), motion(motion_) { } ~ContinuousCollisionObject() {} /// @brief get the type of the object OBJECT_TYPE getObjectType() const { return cgeom->getObjectType(); } /// @brief get the node type NODE_TYPE getNodeType() const { return cgeom->getNodeType(); } /// @brief get the AABB in the world space for the motion inline const AABB& getAABB() const { return aabb; } /// @brief compute the AABB in the world space for the motion inline void computeAABB() { IVector3 box; TMatrix3 R; TVector3 T; motion->getTaylorModel(R, T); Vec3f p = cgeom->aabb_local.min_; box = (R * p + T).getTightBound(); p[2] = cgeom->aabb_local.max_[2]; box = bound(box, (R * p + T).getTightBound()); p[1] = cgeom->aabb_local.max_[1]; p[2] = cgeom->aabb_local.min_[2]; box = bound(box, (R * p + T).getTightBound()); p[2] = cgeom->aabb_local.max_[2]; box = bound(box, (R * p + T).getTightBound()); p[0] = cgeom->aabb_local.max_[0]; p[1] = cgeom->aabb_local.min_[1]; p[2] = cgeom->aabb_local.min_[2]; box = bound(box, (R * p + T).getTightBound()); p[2] = cgeom->aabb_local.max_[2]; box = bound(box, (R * p + T).getTightBound()); p[1] = cgeom->aabb_local.max_[1]; p[2] = cgeom->aabb_local.min_[2]; box = bound(box, (R * p + T).getTightBound()); p[2] = cgeom->aabb_local.max_[2]; box = bound(box, (R * p + T).getTightBound()); aabb.min_ = box.getLow(); aabb.max_ = box.getHigh(); } /// @brief get user data in object void* getUserData() const { return user_data; } /// @brief set user data in object void setUserData(void* data) { user_data = data; } /// @brief get motion from the object instance inline MotionBase* getMotion() const { return motion.get(); } /// @brief get geometry from the object instance FCL_DEPRECATED inline const CollisionGeometry* getCollisionGeometry() const { return cgeom.get(); } /// @brief get geometry from the object instance inline const std::shared_ptr& collisionGeometry() const { return cgeom_const; } protected: std::shared_ptr cgeom; std::shared_ptr cgeom_const; std::shared_ptr motion; /// @brief AABB in the global coordinate for the motion mutable AABB aabb; /// @brief pointer to user defined data specific to this object void* user_data; }; } #endif fcl-0.5.0/include/fcl/config.h.in000066400000000000000000000053361274356571000165130ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2012-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ #ifndef FCL_CONFIG_ #define FCL_CONFIG_ #define FCL_VERSION "@FCL_VERSION@" #define FCL_MAJOR_VERSION @FCL_MAJOR_VERSION@ #define FCL_MINOR_VERSION @FCL_MINOR_VERSION@ #define FCL_PATCH_VERSION @FCL_PATCH_VERSION@ #cmakedefine01 FCL_HAVE_SSE #cmakedefine01 FCL_HAVE_OCTOMAP #cmakedefine01 FCL_HAVE_TINYXML #cmakedefine01 FCL_BUILD_TYPE_DEBUG #cmakedefine01 FCL_BUILD_TYPE_RELEASE #if FCL_HAVE_OCTOMAP #define OCTOMAP_MAJOR_VERSION @OCTOMAP_MAJOR_VERSION@ #define OCTOMAP_MINOR_VERSION @OCTOMAP_MINOR_VERSION@ #define OCTOMAP_PATCH_VERSION @OCTOMAP_PATCH_VERSION@ #define OCTOMAP_VERSION_AT_LEAST(x,y,z) \ (OCTOMAP_MAJOR_VERSION > x || (OCTOMAP_MAJOR_VERSION >= x && \ (OCTOMAP_MINOR_VERSION > y || (OCTOMAP_MINOR_VERSION >= y && \ OCTOMAP_PATCH_VERSION >= z)))) #define OCTOMAP_VERSION_AT_MOST(x,y,z) \ (OCTOMAP_MAJOR_VERSION < x || (OCTOMAP_MAJOR_VERSION <= x && \ (OCTOMAP_MINOR_VERSION < y || (OCTOMAP_MINOR_VERSION <= y && \ OCTOMAP_PATCH_VERSION <= z)))) #endif // FCL_HAVE_OCTOMAP #endif fcl-0.5.0/include/fcl/continuous_collision.h000066400000000000000000000021451274356571000211150ustar00rootroot00000000000000#ifndef FCL_CONTINUOUS_COLLISION_H #define FCL_CONTINUOUS_COLLISION_H #include "fcl/math/vec_3f.h" #include "fcl/collision_object.h" #include "fcl/collision_data.h" namespace fcl { /// @brief continous collision checking between two objects FCL_REAL continuousCollide(const CollisionGeometry* o1, const Transform3f& tf1_beg, const Transform3f& tf1_end, const CollisionGeometry* o2, const Transform3f& tf2_beg, const Transform3f& tf2_end, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result); FCL_REAL continuousCollide(const CollisionObject* o1, const Transform3f& tf1_end, const CollisionObject* o2, const Transform3f& tf2_end, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result); FCL_REAL collide(const ContinuousCollisionObject* o1, const ContinuousCollisionObject* o2, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result); } #endif fcl-0.5.0/include/fcl/data_types.h000066400000000000000000000052441274356571000167740ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_DATA_TYPES_H #define FCL_DATA_TYPES_H #include #include namespace fcl { typedef double FCL_REAL; typedef std::int64_t FCL_INT64; typedef std::uint64_t FCL_UINT64; typedef std::int32_t FCL_INT32; typedef std::uint32_t FCL_UINT32; /// @brief Triangle with 3 indices for points class Triangle { /// @brief indices for each vertex of triangle std::size_t vids[3]; public: /// @brief Default constructor Triangle() {} /// @brief Create a triangle with given vertex indices Triangle(std::size_t p1, std::size_t p2, std::size_t p3) { set(p1, p2, p3); } /// @brief Set the vertex indices of the triangle inline void set(std::size_t p1, std::size_t p2, std::size_t p3) { vids[0] = p1; vids[1] = p2; vids[2] = p3; } /// @access the triangle index inline std::size_t operator[](int i) const { return vids[i]; } inline std::size_t& operator[](int i) { return vids[i]; } }; } #endif fcl-0.5.0/include/fcl/deprecated.h000066400000000000000000000051511274356571000167340ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2013-2016, CNRS-LAAS and AIST * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of CNRS-LAAS and AIST 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 OWNER 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. */ /// \author Florent Lamiraux #ifndef FCL_DEPRECATED_HH # define FCL_DEPRECATED_HH // Define a suffix which can be used to tag a type, a function or a a // variable as deprecated (i.e. it will emit a warning when using it). // // Tagging a function as deprecated: // FCL_DEPRECATED void foo (); // // Tagging a type as deprecated: // FCL_DEPRECATED class Foo {}; // // Tagging a variable as deprecated: // FCL_DEPRECATED int a = 0; // // The use of a macro is required as this is /not/ a standardized // feature of C++ language or preprocessor, even if most of the // compilers support it. # ifdef __GNUC__ # define FCL_DEPRECATED __attribute__ ((deprecated)) # elif defined _MSC_VER # define FCL_DEPRECATED __declspec (deprecated) # elif defined(clang) # define FL_DEPRECATED \ attribute((deprecated("FCL: Use of this method is deprecated"))) # else // If the compiler is not recognized, drop the feature. # define FCL_DEPRECATED /* nothing */ # endif #endif //! FCL_DEPRECATED_HH fcl-0.5.0/include/fcl/distance.h000066400000000000000000000047671274356571000164420ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_DISTANCE_H #define FCL_DISTANCE_H #include "fcl/collision_object.h" #include "fcl/collision_data.h" namespace fcl { /// @brief Main distance interface: given two collision objects, and the requirements for contacts, including whether return the nearest points, this function performs the distance between them. /// Return value is the minimum distance generated between the two objects. FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const DistanceRequest& request, DistanceResult& result); FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const DistanceRequest& request, DistanceResult& result); } #endif fcl-0.5.0/include/fcl/distance_func_matrix.h000066400000000000000000000056411274356571000210310ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_DISTANCE_FUNC_MATRIX_H #define FCL_DISTANCE_FUNC_MATRIX_H #include "fcl/collision_object.h" #include "fcl/collision_data.h" namespace fcl { /// @brief distance matrix stores the functions for distance between different types of objects and provides a uniform call interface template struct DistanceFunctionMatrix { /// @brief the uniform call interface for distance: for distance, we need know /// 1. two objects o1 and o2 and their configuration in world coordinate tf1 and tf2; /// 2. the solver for narrow phase collision, this is for distance computation between geometric shapes; /// 3. the request setting for distance (e.g., whether need to return nearest points); typedef FCL_REAL (*DistanceFunc)(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const DistanceRequest& request, DistanceResult& result); /// @brief each item in the distance matrix is a function to handle distance between objects of type1 and type2 DistanceFunc distance_matrix[NODE_COUNT][NODE_COUNT]; DistanceFunctionMatrix(); }; } #endif fcl-0.5.0/include/fcl/exception.h000066400000000000000000000010701274356571000166260ustar00rootroot00000000000000#ifndef FCL_EXCEPTION_H #define FCL_EXCEPTION_H #include #include namespace fcl { class Exception : public std::runtime_error { public: /** \brief This is just a wrapper on std::runtime_error */ explicit Exception(const std::string& what) : std::runtime_error(what) { } /** \brief This is just a wrapper on std::runtime_error with a prefix added */ Exception(const std::string &prefix, const std::string& what) : std::runtime_error(prefix + ": " + what) { } virtual ~Exception(void) throw() { } }; } #endif fcl-0.5.0/include/fcl/intersect.h000066400000000000000000000424701274356571000166410ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_INTERSECT_H #define FCL_INTERSECT_H #include "fcl/math/transform.h" namespace fcl { /// @brief A class solves polynomial degree (1,2,3) equations class PolySolver { public: /// @brief Solve a linear equation with coefficients c, return roots s and number of roots static int solveLinear(FCL_REAL c[2], FCL_REAL s[1]); /// @brief Solve a quadratic function with coefficients c, return roots s and number of roots static int solveQuadric(FCL_REAL c[3], FCL_REAL s[2]); /// @brief Solve a cubic function with coefficients c, return roots s and number of roots static int solveCubic(FCL_REAL c[4], FCL_REAL s[3]); private: /// @brief Check whether v is zero static inline bool isZero(FCL_REAL v); /// @brief Compute v^{1/3} static inline bool cbrt(FCL_REAL v); static const FCL_REAL NEAR_ZERO_THRESHOLD; }; /// @brief CCD intersect kernel among primitives class Intersect { public: /// @brief CCD intersect between one vertex and one face /// [a0, b0, c0] and [a1, b1, c1] are points for the triangle face in time t0 and t1 /// p0 and p1 are points for vertex in time t0 and t1 /// p_i returns the coordinate of the collision point static bool intersect_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0, const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1, FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true); /// @brief CCD intersect between two edges /// [a0, b0] and [a1, b1] are points for one edge in time t0 and t1 /// [c0, d0] and [c1, d1] are points for the other edge in time t0 and t1 /// p_i returns the coordinate of the collision point static bool intersect_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0, const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1, FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true); /// @brief CCD intersect between one vertex and one face, using additional filter static bool intersect_VF_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0, const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1, FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true); /// @brief CCD intersect between two edges, using additional filter static bool intersect_EE_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0, const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1, FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true); /// @brief CCD intersect between one vertex and and one edge static bool intersect_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0, const Vec3f& a1, const Vec3f& b1, const Vec3f& p1, const Vec3f& L); /// @brief CD intersect between two triangles [P1, P2, P3] and [Q1, Q2, Q3] static bool intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3, Vec3f* contact_points = NULL, unsigned int* num_contact_points = NULL, FCL_REAL* penetration_depth = NULL, Vec3f* normal = NULL); static bool intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3, const Matrix3f& R, const Vec3f& T, Vec3f* contact_points = NULL, unsigned int* num_contact_points = NULL, FCL_REAL* penetration_depth = NULL, Vec3f* normal = NULL); static bool intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3, const Transform3f& tf, Vec3f* contact_points = NULL, unsigned int* num_contact_points = NULL, FCL_REAL* penetration_depth = NULL, Vec3f* normal = NULL); private: /// @brief Project function used in intersect_Triangle() static int project6(const Vec3f& ax, const Vec3f& p1, const Vec3f& p2, const Vec3f& p3, const Vec3f& q1, const Vec3f& q2, const Vec3f& q3); /// @brief Check whether one value is zero static inline bool isZero(FCL_REAL v); /// @brief Solve the cubic function using Newton method, also satisfies the interval restriction static bool solveCubicWithIntervalNewton(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0, const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd, FCL_REAL& l, FCL_REAL& r, bool bVF, FCL_REAL coeffs[], Vec3f* data = NULL); /// @brief Check whether one point p is within triangle [a, b, c] static bool insideTriangle(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f&p); /// @brief Check whether one point p is within a line segment [a, b] static bool insideLineSegment(const Vec3f& a, const Vec3f& b, const Vec3f& p); /// @brief Calculate the line segment papb that is the shortest route between /// two lines p1p2 and p3p4. Calculate also the values of mua and mub where /// pa = p1 + mua (p2 - p1) /// pb = p3 + mub (p4 - p3) /// return FALSE if no solution exists. static bool linelineIntersect(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3, const Vec3f& p4, Vec3f* pa, Vec3f* pb, FCL_REAL* mua, FCL_REAL* mub); /// @brief Check whether a root for VF intersection is valid (i.e. within the triangle at intersection t static bool checkRootValidity_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0, const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp, FCL_REAL t); /// @brief Check whether a root for EE intersection is valid (i.e. within the two edges intersected at the given time static bool checkRootValidity_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0, const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd, FCL_REAL t, Vec3f* q_i = NULL); /// @brief Check whether a root for VE intersection is valid static bool checkRootValidity_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0, const Vec3f& va, const Vec3f& vb, const Vec3f& vp, FCL_REAL t); /// @brief Solve a square function for EE intersection (with interval restriction) static bool solveSquare(FCL_REAL a, FCL_REAL b, FCL_REAL c, const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0, const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd, bool bVF, FCL_REAL* ret); /// @brief Solve a square function for VE intersection (with interval restriction) static bool solveSquare(FCL_REAL a, FCL_REAL b, FCL_REAL c, const Vec3f& a0, const Vec3f& b0, const Vec3f& p0, const Vec3f& va, const Vec3f& vb, const Vec3f& vp); /// @brief Compute the cubic coefficients for VF intersection /// See Paper "Interactive Continuous Collision Detection between Deformable Models using Connectivity-Based Culling", Equation 1. static void computeCubicCoeff_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0, const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp, FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d); /// @brief Compute the cubic coefficients for EE intersection static void computeCubicCoeff_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0, const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd, FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d); /// @brief Compute the cubic coefficients for VE intersection static void computeCubicCoeff_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0, const Vec3f& va, const Vec3f& vb, const Vec3f& vp, const Vec3f& L, FCL_REAL* a, FCL_REAL* b, FCL_REAL* c); /// @brief filter for intersection, works for both VF and EE static bool intersectPreFiltering(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0, const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1); /// @brief distance of point v to a plane n * x - t = 0 static FCL_REAL distanceToPlane(const Vec3f& n, FCL_REAL t, const Vec3f& v); /// @brief check wether points v1, v2, v2 are on the same side of plane n * x - t = 0 static bool sameSideOfPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, const Vec3f& n, FCL_REAL t); /// @brief clip triangle v1, v2, v3 by the prism made by t1, t2 and t3. The normal of the prism is tn and is cutted up by to static void clipTriangleByTriangleAndEdgePlanes(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, const Vec3f& t1, const Vec3f& t2, const Vec3f& t3, const Vec3f& tn, FCL_REAL to, Vec3f clipped_points[], unsigned int* num_clipped_points, bool clip_triangle = false); /// @brief build a plane passed through triangle v1 v2 v3 static bool buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, Vec3f* n, FCL_REAL* t); /// @brief build a plane pass through edge v1 and v2, normal is tn static bool buildEdgePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& tn, Vec3f* n, FCL_REAL* t); /// @brief compute the points which has deepest penetration depth static void computeDeepestPoints(Vec3f* clipped_points, unsigned int num_clipped_points, const Vec3f& n, FCL_REAL t, FCL_REAL* penetration_depth, Vec3f* deepest_points, unsigned int* num_deepest_points); /// @brief clip polygon by plane static void clipPolygonByPlane(Vec3f* polygon_points, unsigned int num_polygon_points, const Vec3f& n, FCL_REAL t, Vec3f clipped_points[], unsigned int* num_clipped_points); /// @brief clip a line segment by plane static void clipSegmentByPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& n, FCL_REAL t, Vec3f* clipped_point); /// @brief compute the cdf(x) static FCL_REAL gaussianCDF(FCL_REAL x) { return 0.5 * std::erfc(-x / sqrt(2.0)); } static const FCL_REAL EPSILON; static const FCL_REAL NEAR_ZERO_THRESHOLD; static const FCL_REAL CCD_RESOLUTION; static const unsigned int MAX_TRIANGLE_CLIPS = 8; }; /// @brief Project functions class Project { public: struct ProjectResult { /// @brief Parameterization of the projected point (based on the simplex to be projected, use 2 or 3 or 4 of the array) FCL_REAL parameterization[4]; /// @brief square distance from the query point to the projected simplex FCL_REAL sqr_distance; /// @brief the code of the projection type unsigned int encode; ProjectResult() : sqr_distance(-1), encode(0) { } }; /// @brief Project point p onto line a-b static ProjectResult projectLine(const Vec3f& a, const Vec3f& b, const Vec3f& p); /// @brief Project point p onto triangle a-b-c static ProjectResult projectTriangle(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& p); /// @brief Project point p onto tetrahedra a-b-c-d static ProjectResult projectTetrahedra(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& d, const Vec3f& p); /// @brief Project origin (0) onto line a-b static ProjectResult projectLineOrigin(const Vec3f& a, const Vec3f& b); /// @brief Project origin (0) onto triangle a-b-c static ProjectResult projectTriangleOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c); /// @brief Project origin (0) onto tetrahedran a-b-c-d static ProjectResult projectTetrahedraOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& d); }; /// @brief Triangle distance functions class TriangleDistance { public: /// @brief Returns closest points between an segment pair. /// The first segment is P + t * A /// The second segment is Q + t * B /// X, Y are the closest points on the two segments /// VEC is the vector between X and Y static void segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, const Vec3f& B, Vec3f& VEC, Vec3f& X, Vec3f& Y); /// @brief Compute the closest points on two triangles given their absolute coordinate, and returns the distance between them /// S and T are two triangles /// If the triangles are disjoint, P and Q give the closet points of S and T respectively. However, /// if the triangles overlap, P and Q are basically a random pair of points from the triangles, not /// coincident points on the intersection of the triangles, as might be expected. static FCL_REAL triDistance(const Vec3f S[3], const Vec3f T[3], Vec3f& P, Vec3f& Q); static FCL_REAL triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3, const Vec3f& T1, const Vec3f& T2, const Vec3f& T3, Vec3f& P, Vec3f& Q); /// @brief Compute the closest points on two triangles given the relative transform between them, and returns the distance between them /// S and T are two triangles /// If the triangles are disjoint, P and Q give the closet points of S and T respectively. However, /// if the triangles overlap, P and Q are basically a random pair of points from the triangles, not /// coincident points on the intersection of the triangles, as might be expected. /// The returned P and Q are both in the coordinate of the first triangle's coordinate static FCL_REAL triDistance(const Vec3f S[3], const Vec3f T[3], const Matrix3f& R, const Vec3f& Tl, Vec3f& P, Vec3f& Q); static FCL_REAL triDistance(const Vec3f S[3], const Vec3f T[3], const Transform3f& tf, Vec3f& P, Vec3f& Q); static FCL_REAL triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3, const Vec3f& T1, const Vec3f& T2, const Vec3f& T3, const Matrix3f& R, const Vec3f& Tl, Vec3f& P, Vec3f& Q); static FCL_REAL triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3, const Vec3f& T1, const Vec3f& T2, const Vec3f& T3, const Transform3f& tf, Vec3f& P, Vec3f& Q); }; } #endif fcl-0.5.0/include/fcl/learning/000077500000000000000000000000001274356571000162605ustar00rootroot00000000000000fcl-0.5.0/include/fcl/learning/classifier.h000066400000000000000000000063421274356571000205620ustar00rootroot00000000000000#ifndef FCL_LEARNING_CLASSIFIER_H #define FCL_LEARNING_CLASSIFIER_H #include "fcl/math/vec_nf.h" namespace fcl { template struct Item { Vecnf q; bool label; FCL_REAL w; Item(const Vecnf& q_, bool label_, FCL_REAL w_ = 1) : q(q_), label(label_), w(w_) {} Item() {} }; template struct Scaler { Vecnf v_min, v_max; Scaler() { // default no scale for(std::size_t i = 0; i < N; ++i) { v_min[i] = 0; v_max[i] = 1; } } Scaler(const Vecnf& v_min_, const Vecnf& v_max_) : v_min(v_min_), v_max(v_max_) {} Vecnf scale(const Vecnf& v) const { Vecnf res; for(std::size_t i = 0; i < N; ++i) res[i] = (v[i] - v_min[i]) / (v_max[i] - v_min[i]); return res; } Vecnf unscale(const Vecnf& v) const { Vecnf res; for(std::size_t i = 0; i < N; ++i) res[i] = v[i] * (v_max[i] - v_min[i]) + v_min[i]; return res; } }; struct PredictResult { bool label; FCL_REAL prob; PredictResult() {} PredictResult(bool label_, FCL_REAL prob_ = 1) : label(label_), prob(prob_) {} }; template class SVMClassifier { public: ~SVMClassifier() {} virtual PredictResult predict(const Vecnf& q) const = 0; virtual std::vector predict(const std::vector >& qs) const = 0; virtual std::vector > getSupportVectors() const = 0; virtual void setScaler(const Scaler& scaler) = 0; virtual void learn(const std::vector >& data) = 0; FCL_REAL error_rate(const std::vector >& data) const { std::size_t num = data.size(); std::size_t error_num = 0; for(std::size_t i = 0; i < data.size(); ++i) { PredictResult res = predict(data[i].q); if(res.label != data[i].label) error_num++; } return error_num / (FCL_REAL)num; } }; template Scaler computeScaler(const std::vector >& data) { Vecnf lower_bound, upper_bound; for(std::size_t j = 0; j < N; ++j) { lower_bound[j] = std::numeric_limits::max(); upper_bound[j] = -std::numeric_limits::max(); } for(std::size_t i = 0; i < data.size(); ++i) { for(std::size_t j = 0; j < N; ++j) { if(data[i].q[j] < lower_bound[j]) lower_bound[j] = data[i].q[j]; if(data[i].q[j] > upper_bound[j]) upper_bound[j] = data[i].q[j]; } } return Scaler(lower_bound, upper_bound); } template Scaler computeScaler(const std::vector >& data) { Vecnf lower_bound, upper_bound; for(std::size_t j = 0; j < N; ++j) { lower_bound[j] = std::numeric_limits::max(); upper_bound[j] = -std::numeric_limits::max(); } for(std::size_t i = 0; i < data.size(); ++i) { for(std::size_t j = 0; j < N; ++j) { if(data[i][j] < lower_bound[j]) lower_bound[j] = data[i][j]; if(data[i][j] > upper_bound[j]) upper_bound[j] = data[i][j]; } } return Scaler(lower_bound, upper_bound); } } #endif fcl-0.5.0/include/fcl/math/000077500000000000000000000000001274356571000154125ustar00rootroot00000000000000fcl-0.5.0/include/fcl/math/constants.h000066400000000000000000000041201274356571000175740ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2016, Rice University * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Rice University 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 OWNER 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. */ /** \author Mark Moll */ #ifndef FCL_MATH_CONSTANTS_ #define FCL_MATH_CONSTANTS_ #include "fcl/data_types.h" namespace fcl { namespace constants { /// The mathematical constant pi constexpr FCL_REAL pi = FCL_REAL(3.141592653589793238462643383279502884197169399375105820974944592L); /// The golden ratio constexpr FCL_REAL phi = FCL_REAL(1.618033988749894848204586834365638117720309179805762862135448623L); } } #endif fcl-0.5.0/include/fcl/math/math_details.h000066400000000000000000000362701274356571000202310ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ #ifndef FCL_MATH_DETAILS_H #define FCL_MATH_DETAILS_H #include #include #include namespace fcl { namespace details { template struct Vec3Data { typedef T meta_type; T vs[3]; Vec3Data() { setValue(0); } Vec3Data(T x) { setValue(x); } Vec3Data(T* x) { memcpy(vs, x, sizeof(T) * 3); } Vec3Data(T x, T y, T z) { setValue(x, y, z); } inline void setValue(T x, T y, T z) { vs[0] = x; vs[1] = y; vs[2] = z; } inline void setValue(T x) { vs[0] = x; vs[1] = x; vs[2] = x; } inline void negate() { vs[0] = -vs[0]; vs[1] = -vs[1]; vs[2] = -vs[2]; } inline Vec3Data& ubound(const Vec3Data& u) { vs[0] = std::min(vs[0], u.vs[0]); vs[1] = std::min(vs[1], u.vs[1]); vs[2] = std::min(vs[2], u.vs[2]); return *this; } inline Vec3Data& lbound(const Vec3Data& l) { vs[0] = std::max(vs[0], l.vs[0]); vs[1] = std::max(vs[1], l.vs[1]); vs[2] = std::max(vs[2], l.vs[2]); return *this; } T operator [] (size_t i) const { return vs[i]; } T& operator [] (size_t i) { return vs[i]; } inline Vec3Data operator + (const Vec3Data& other) const { return Vec3Data(vs[0] + other.vs[0], vs[1] + other.vs[1], vs[2] + other.vs[2]); } inline Vec3Data operator - (const Vec3Data& other) const { return Vec3Data(vs[0] - other.vs[0], vs[1] - other.vs[1], vs[2] - other.vs[2]); } inline Vec3Data operator * (const Vec3Data& other) const { return Vec3Data(vs[0] * other.vs[0], vs[1] * other.vs[1], vs[2] * other.vs[2]); } inline Vec3Data operator / (const Vec3Data& other) const { return Vec3Data(vs[0] / other.vs[0], vs[1] / other.vs[1], vs[2] / other.vs[2]); } inline Vec3Data& operator += (const Vec3Data& other) { vs[0] += other.vs[0]; vs[1] += other.vs[1]; vs[2] += other.vs[2]; return *this; } inline Vec3Data& operator -= (const Vec3Data& other) { vs[0] -= other.vs[0]; vs[1] -= other.vs[1]; vs[2] -= other.vs[2]; return *this; } inline Vec3Data& operator *= (const Vec3Data& other) { vs[0] *= other.vs[0]; vs[1] *= other.vs[1]; vs[2] *= other.vs[2]; return *this; } inline Vec3Data& operator /= (const Vec3Data& other) { vs[0] /= other.vs[0]; vs[1] /= other.vs[1]; vs[2] /= other.vs[2]; return *this; } inline Vec3Data operator + (T t) const { return Vec3Data(vs[0] + t, vs[1] + t, vs[2] + t); } inline Vec3Data operator - (T t) const { return Vec3Data(vs[0] - t, vs[1] - t, vs[2] - t); } inline Vec3Data operator * (T t) const { return Vec3Data(vs[0] * t, vs[1] * t, vs[2] * t); } inline Vec3Data operator / (T t) const { T inv_t = 1.0 / t; return Vec3Data(vs[0] * inv_t, vs[1] * inv_t, vs[2] * inv_t); } inline Vec3Data& operator += (T t) { vs[0] += t; vs[1] += t; vs[2] += t; return *this; } inline Vec3Data& operator -= (T t) { vs[0] -= t; vs[1] -= t; vs[2] -= t; return *this; } inline Vec3Data& operator *= (T t) { vs[0] *= t; vs[1] *= t; vs[2] *= t; return *this; } inline Vec3Data& operator /= (T t) { T inv_t = 1.0 / t; vs[0] *= inv_t; vs[1] *= inv_t; vs[2] *= inv_t; return *this; } inline Vec3Data operator - () const { return Vec3Data(-vs[0], -vs[1], -vs[2]); } }; template static inline Vec3Data cross_prod(const Vec3Data& l, const Vec3Data& r) { return Vec3Data(l.vs[1] * r.vs[2] - l.vs[2] * r.vs[1], l.vs[2] * r.vs[0] - l.vs[0] * r.vs[2], l.vs[0] * r.vs[1] - l.vs[1] * r.vs[0]); } template static inline T dot_prod3(const Vec3Data& l, const Vec3Data& r) { return l.vs[0] * r.vs[0] + l.vs[1] * r.vs[1] + l.vs[2] * r.vs[2]; } template static inline Vec3Data min(const Vec3Data& x, const Vec3Data& y) { return Vec3Data(std::min(x.vs[0], y.vs[0]), std::min(x.vs[1], y.vs[1]), std::min(x.vs[2], y.vs[2])); } template static inline Vec3Data max(const Vec3Data& x, const Vec3Data& y) { return Vec3Data(std::max(x.vs[0], y.vs[0]), std::max(x.vs[1], y.vs[1]), std::max(x.vs[2], y.vs[2])); } template static inline Vec3Data abs(const Vec3Data& x) { return Vec3Data(std::abs(x.vs[0]), std::abs(x.vs[1]), std::abs(x.vs[2])); } template static inline bool equal(const Vec3Data& x, const Vec3Data& y, T epsilon) { return ((x.vs[0] - y.vs[0] < epsilon) && (x.vs[0] - y.vs[0] > -epsilon) && (x.vs[1] - y.vs[1] < epsilon) && (x.vs[1] - y.vs[1] > -epsilon) && (x.vs[2] - y.vs[2] < epsilon) && (x.vs[2] - y.vs[2] > -epsilon)); } template struct Matrix3Data { typedef T meta_type; typedef Vec3Data vector_type; Vec3Data rs[3]; Matrix3Data() {}; Matrix3Data(T xx, T xy, T xz, T yx, T yy, T yz, T zx, T zy, T zz) { setValue(xx, xy, xz, yx, yy, yz, zx, zy, zz); } Matrix3Data(const Vec3Data& v1, const Vec3Data& v2, const Vec3Data& v3) { rs[0] = v1; rs[1] = v2; rs[2] = v3; } Matrix3Data(const Matrix3Data& other) { rs[0] = other.rs[0]; rs[1] = other.rs[1]; rs[2] = other.rs[2]; } inline Vec3Data getColumn(size_t i) const { return Vec3Data(rs[0][i], rs[1][i], rs[2][i]); } inline const Vec3Data& getRow(size_t i) const { return rs[i]; } inline T operator() (size_t i, size_t j) const { return rs[i][j]; } inline T& operator() (size_t i, size_t j) { return rs[i][j]; } inline Vec3Data operator * (const Vec3Data& v) const { return Vec3Data(dot_prod3(rs[0], v), dot_prod3(rs[1], v), dot_prod3(rs[2], v)); } inline Matrix3Data operator * (const Matrix3Data& other) const { return Matrix3Data(other.transposeDotX(rs[0]), other.transposeDotY(rs[0]), other.transposeDotZ(rs[0]), other.transposeDotX(rs[1]), other.transposeDotY(rs[1]), other.transposeDotZ(rs[1]), other.transposeDotX(rs[2]), other.transposeDotY(rs[2]), other.transposeDotZ(rs[2])); } inline Matrix3Data operator + (const Matrix3Data& other) const { return Matrix3Data(rs[0] + other.rs[0], rs[1] + other.rs[1], rs[2] + other.rs[2]); } inline Matrix3Data operator - (const Matrix3Data& other) const { return Matrix3Data(rs[0] - other.rs[0], rs[1] - other.rs[1], rs[2] - other.rs[2]); } inline Matrix3Data operator + (T c) const { return Matrix3Data(rs[0] + c, rs[1] + c, rs[2] + c); } inline Matrix3Data operator - (T c) const { return Matrix3Data(rs[0] - c, rs[1] - c, rs[2] - c); } inline Matrix3Data operator * (T c) const { return Matrix3Data(rs[0] * c, rs[1] * c, rs[2] * c); } inline Matrix3Data operator / (T c) const { return Matrix3Data(rs[0] / c, rs[1] / c, rs[2] / c); } inline Matrix3Data& operator *= (const Matrix3Data& other) { rs[0].setValue(other.transposeDotX(rs[0]), other.transposeDotY(rs[0]), other.transposeDotZ(rs[0])); rs[1].setValue(other.transposeDotX(rs[1]), other.transposeDotY(rs[1]), other.transposeDotZ(rs[1])); rs[2].setValue(other.transposeDotX(rs[2]), other.transposeDotY(rs[2]), other.transposeDotZ(rs[2])); return *this; } inline Matrix3Data& operator += (const Matrix3Data& other) { rs[0] += other.rs[0]; rs[1] += other.rs[1]; rs[2] += other.rs[2]; return *this; } inline Matrix3Data& operator -= (const Matrix3Data& other) { rs[0] -= other.rs[0]; rs[1] -= other.rs[1]; rs[2] -= other.rs[2]; return *this; } inline Matrix3Data& operator += (T c) { rs[0] += c; rs[1] += c; rs[2] += c; return *this; } inline Matrix3Data& operator - (T c) { rs[0] -= c; rs[1] -= c; rs[2] -= c; return *this; } inline Matrix3Data& operator * (T c) { rs[0] *= c; rs[1] *= c; rs[2] *= c; return *this; } inline Matrix3Data& operator / (T c) { rs[0] /= c; rs[1] /= c; rs[2] /= c; return *this; } void setIdentity() { setValue((T)1, (T)0, (T)0, (T)0, (T)1, (T)0, (T)0, (T)0, (T)1); } void setZero() { setValue((T)0); } static const Matrix3Data& getIdentity() { static const Matrix3Data I((T)1, (T)0, (T)0, (T)0, (T)1, (T)0, (T)0, (T)0, (T)1); return I; } T determinant() const { return dot_prod3(rs[0], cross_prod(rs[1], rs[2])); } Matrix3Data& transpose() { register T tmp = rs[0][1]; rs[0][1] = rs[1][0]; rs[1][0] = tmp; tmp = rs[0][2]; rs[0][2] = rs[2][0]; rs[2][0] = tmp; tmp = rs[2][1]; rs[2][1] = rs[1][2]; rs[1][2] = tmp; return *this; } Matrix3Data& inverse() { T det = determinant(); register T inrsdet = 1 / det; setValue((rs[1][1] * rs[2][2] - rs[1][2] * rs[2][1]) * inrsdet, (rs[0][2] * rs[2][1] - rs[0][1] * rs[2][2]) * inrsdet, (rs[0][1] * rs[1][2] - rs[0][2] * rs[1][1]) * inrsdet, (rs[1][2] * rs[2][0] - rs[1][0] * rs[2][2]) * inrsdet, (rs[0][0] * rs[2][2] - rs[0][2] * rs[2][0]) * inrsdet, (rs[0][2] * rs[1][0] - rs[0][0] * rs[1][2]) * inrsdet, (rs[1][0] * rs[2][1] - rs[1][1] * rs[2][0]) * inrsdet, (rs[0][1] * rs[2][0] - rs[0][0] * rs[2][1]) * inrsdet, (rs[0][0] * rs[1][1] - rs[0][1] * rs[1][0]) * inrsdet); return *this; } Matrix3Data transposeTimes(const Matrix3Data& m) const { return Matrix3Data(rs[0][0] * m.rs[0][0] + rs[1][0] * m.rs[1][0] + rs[2][0] * m.rs[2][0], rs[0][0] * m.rs[0][1] + rs[1][0] * m.rs[1][1] + rs[2][0] * m.rs[2][1], rs[0][0] * m.rs[0][2] + rs[1][0] * m.rs[1][2] + rs[2][0] * m.rs[2][2], rs[0][1] * m.rs[0][0] + rs[1][1] * m.rs[1][0] + rs[2][1] * m.rs[2][0], rs[0][1] * m.rs[0][1] + rs[1][1] * m.rs[1][1] + rs[2][1] * m.rs[2][1], rs[0][1] * m.rs[0][2] + rs[1][1] * m.rs[1][2] + rs[2][1] * m.rs[2][2], rs[0][2] * m.rs[0][0] + rs[1][2] * m.rs[1][0] + rs[2][2] * m.rs[2][0], rs[0][2] * m.rs[0][1] + rs[1][2] * m.rs[1][1] + rs[2][2] * m.rs[2][1], rs[0][2] * m.rs[0][2] + rs[1][2] * m.rs[1][2] + rs[2][2] * m.rs[2][2]); } Matrix3Data timesTranspose(const Matrix3Data& m) const { return Matrix3Data(dot_prod3(rs[0], m[0]), dot_prod3(rs[0], m[1]), dot_prod3(rs[0], m[2]), dot_prod3(rs[1], m[0]), dot_prod3(rs[1], m[1]), dot_prod3(rs[1], m[2]), dot_prod3(rs[2], m[0]), dot_prod3(rs[2], m[1]), dot_prod3(rs[2], m[2])); } Vec3Data transposeTimes(const Vec3Data& v) const { return Vec3Data(transposeDotX(v), transposeDotY(v), transposeDotZ(v)); } inline T transposeDotX(const Vec3Data& v) const { return rs[0][0] * v[0] + rs[1][0] * v[1] + rs[2][0] * v[2]; } inline T transposeDotY(const Vec3Data& v) const { return rs[0][1] * v[0] + rs[1][1] * v[1] + rs[2][1] * v[2]; } inline T transposeDotZ(const Vec3Data& v) const { return rs[0][2] * v[0] + rs[1][2] * v[1] + rs[2][2] * v[2]; } inline T transposeDot(size_t i, const Vec3Data& v) const { return rs[0][i] * v[0] + rs[1][i] * v[1] + rs[2][i] * v[2]; } inline T dotX(const Vec3Data& v) const { return rs[0][0] * v[0] + rs[0][1] * v[1] + rs[0][2] * v[2]; } inline T dotY(const Vec3Data& v) const { return rs[1][0] * v[0] + rs[1][1] * v[1] + rs[1][2] * v[2]; } inline T dotZ(const Vec3Data& v) const { return rs[2][0] * v[0] + rs[2][1] * v[1] + rs[2][2] * v[2]; } inline T dot(size_t i, const Vec3Data& v) const { return rs[i][0] * v[0] + rs[i][1] * v[1] + rs[i][2] * v[2]; } inline void setValue(T xx, T xy, T xz, T yx, T yy, T yz, T zx, T zy, T zz) { rs[0].setValue(xx, xy, xz); rs[1].setValue(yx, yy, yz); rs[2].setValue(zx, zy, zz); } inline void setValue(T x) { rs[0].setValue(x); rs[1].setValue(x); rs[2].setValue(x); } }; template Matrix3Data abs(const Matrix3Data& m) { return Matrix3Data(abs(m.rs[0]), abs(m.rs[1]), abs(m.rs[2])); } template Matrix3Data transpose(const Matrix3Data& m) { return Matrix3Data(m.rs[0][0], m.rs[1][0], m.rs[2][0], m.rs[0][1], m.rs[1][1], m.rs[2][1], m.rs[0][2], m.rs[1][2], m.rs[2][2]); } template Matrix3Data inverse(const Matrix3Data& m) { T det = m.determinant(); T inrsdet = 1 / det; return Matrix3Data((m.rs[1][1] * m.rs[2][2] - m.rs[1][2] * m.rs[2][1]) * inrsdet, (m.rs[0][2] * m.rs[2][1] - m.rs[0][1] * m.rs[2][2]) * inrsdet, (m.rs[0][1] * m.rs[1][2] - m.rs[0][2] * m.rs[1][1]) * inrsdet, (m.rs[1][2] * m.rs[2][0] - m.rs[1][0] * m.rs[2][2]) * inrsdet, (m.rs[0][0] * m.rs[2][2] - m.rs[0][2] * m.rs[2][0]) * inrsdet, (m.rs[0][2] * m.rs[1][0] - m.rs[0][0] * m.rs[1][2]) * inrsdet, (m.rs[1][0] * m.rs[2][1] - m.rs[1][1] * m.rs[2][0]) * inrsdet, (m.rs[0][1] * m.rs[2][0] - m.rs[0][0] * m.rs[2][1]) * inrsdet, (m.rs[0][0] * m.rs[1][1] - m.rs[0][1] * m.rs[1][0]) * inrsdet); } } } #endif fcl-0.5.0/include/fcl/math/matrix_3f.h000066400000000000000000000315321274356571000174630ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_MATRIX_3F_H #define FCL_MATRIX_3F_H #include "fcl/math/vec_3f.h" namespace fcl { /// @brief Matrix2 class wrapper. the core data is in the template parameter class template class Matrix3fX { public: typedef typename T::meta_type U; typedef typename T::vector_type S; T data; Matrix3fX() {} Matrix3fX(U xx, U xy, U xz, U yx, U yy, U yz, U zx, U zy, U zz) : data(xx, xy, xz, yx, yy, yz, zx, zy, zz) {} Matrix3fX(const Vec3fX& v1, const Vec3fX& v2, const Vec3fX& v3) : data(v1.data, v2.data, v3.data) {} Matrix3fX(const Matrix3fX& other) : data(other.data) {} Matrix3fX(const T& data_) : data(data_) {} inline Vec3fX getColumn(size_t i) const { return Vec3fX(data.getColumn(i)); } inline Vec3fX getRow(size_t i) const { return Vec3fX(data.getRow(i)); } inline U operator () (size_t i, size_t j) const { return data(i, j); } inline U& operator () (size_t i, size_t j) { return data(i, j); } inline Vec3fX operator * (const Vec3fX& v) const { return Vec3fX(data * v.data); } inline Matrix3fX operator * (const Matrix3fX& m) const { return Matrix3fX(data * m.data); } inline Matrix3fX operator + (const Matrix3fX& other) const { return Matrix3fX(data + other.data); } inline Matrix3fX operator - (const Matrix3fX& other) const { return Matrix3fX(data - other.data); } inline Matrix3fX operator + (U c) const { return Matrix3fX(data + c); } inline Matrix3fX operator - (U c) const { return Matrix3fX(data - c); } inline Matrix3fX operator * (U c) const { return Matrix3fX(data * c); } inline Matrix3fX operator / (U c) const { return Matrix3fX(data / c); } inline Matrix3fX& operator *= (const Matrix3fX& other) { data *= other.data; return *this; } inline Matrix3fX& operator += (const Matrix3fX& other) { data += other.data; return *this; } inline Matrix3fX& operator -= (const Matrix3fX& other) { data -= other.data; return *this; } inline Matrix3fX& operator += (U c) { data += c; return *this; } inline Matrix3fX& operator -= (U c) { data -= c; return *this; } inline Matrix3fX& operator *= (U c) { data *= c; return *this; } inline Matrix3fX& operator /= (U c) { data /= c; return *this; } inline void setIdentity() { data.setIdentity(); } inline bool isIdentity () const { return data (0,0) == 1 && data (0,1) == 0 && data (0,2) == 0 && data (1,0) == 0 && data (1,1) == 1 && data (1,2) == 0 && data (2,0) == 0 && data (2,1) == 0 && data (2,2) == 1; } inline void setZero() { data.setZero(); } /// @brief Set the matrix from euler angles YPR around ZYX axes /// @param eulerX Roll about X axis /// @param eulerY Pitch around Y axis /// @param eulerZ Yaw aboud Z axis /// /// These angles are used to produce a rotation matrix. The euler /// angles are applied in ZYX order. I.e a vector is first rotated /// about X then Y and then Z inline void setEulerZYX(FCL_REAL eulerX, FCL_REAL eulerY, FCL_REAL eulerZ) { FCL_REAL ci(cos(eulerX)); FCL_REAL cj(cos(eulerY)); FCL_REAL ch(cos(eulerZ)); FCL_REAL si(sin(eulerX)); FCL_REAL sj(sin(eulerY)); FCL_REAL sh(sin(eulerZ)); FCL_REAL cc = ci * ch; FCL_REAL cs = ci * sh; FCL_REAL sc = si * ch; FCL_REAL ss = si * sh; setValue(cj * ch, sj * sc - cs, sj * cc + ss, cj * sh, sj * ss + cc, sj * cs - sc, -sj, cj * si, cj * ci); } /// @brief Set the matrix from euler angles using YPR around YXZ respectively /// @param yaw Yaw about Y axis /// @param pitch Pitch about X axis /// @param roll Roll about Z axis void setEulerYPR(FCL_REAL yaw, FCL_REAL pitch, FCL_REAL roll) { setEulerZYX(roll, pitch, yaw); } inline U determinant() const { return data.determinant(); } Matrix3fX& transpose() { data.transpose(); return *this; } Matrix3fX& inverse() { data.inverse(); return *this; } Matrix3fX& abs() { data.abs(); return *this; } static const Matrix3fX& getIdentity() { static const Matrix3fX I((U)1, (U)0, (U)0, (U)0, (U)1, (U)0, (U)0, (U)0, (U)1); return I; } Matrix3fX transposeTimes(const Matrix3fX& other) const { return Matrix3fX(data.transposeTimes(other.data)); } Matrix3fX timesTranspose(const Matrix3fX& other) const { return Matrix3fX(data.timesTranspose(other.data)); } Vec3fX transposeTimes(const Vec3fX& v) const { return Vec3fX(data.transposeTimes(v.data)); } Matrix3fX tensorTransform(const Matrix3fX& m) const { Matrix3fX res(*this); res *= m; return res.timesTranspose(*this); } inline U transposeDotX(const Vec3fX& v) const { return data.transposeDot(0, v.data); } inline U transposeDotY(const Vec3fX& v) const { return data.transposeDot(1, v.data); } inline U transposeDotZ(const Vec3fX& v) const { return data.transposeDot(2, v.data); } inline U transposeDot(size_t i, const Vec3fX& v) const { return data.transposeDot(i, v.data); } inline U dotX(const Vec3fX& v) const { return data.dot(0, v.data); } inline U dotY(const Vec3fX& v) const { return data.dot(1, v.data); } inline U dotZ(const Vec3fX& v) const { return data.dot(2, v.data); } inline U dot(size_t i, const Vec3fX& v) const { return data.dot(i, v.data); } inline void setValue(U xx, U xy, U xz, U yx, U yy, U yz, U zx, U zy, U zz) { data.setValue(xx, xy, xz, yx, yy, yz, zx, zy, zz); } inline void setValue(U x) { data.setValue(x); } }; template void hat(Matrix3fX& mat, const Vec3fX& vec) { mat.setValue(0, -vec[2], vec[1], vec[2], 0, -vec[0], -vec[1], vec[0], 0); } template void relativeTransform(const Matrix3fX& R1, const Vec3fX& t1, const Matrix3fX& R2, const Vec3fX& t2, Matrix3fX& R, Vec3fX& t) { R = R1.transposeTimes(R2); t = R1.transposeTimes(t2 - t1); } /// @brief compute the eigen vector and eigen vector of a matrix. dout is the eigen values, vout is the eigen vectors template void eigen(const Matrix3fX& m, typename T::meta_type dout[3], Vec3fX vout[3]) { Matrix3fX R(m); int n = 3; int j, iq, ip, i; typename T::meta_type tresh, theta, tau, t, sm, s, h, g, c; int nrot; typename T::meta_type b[3]; typename T::meta_type z[3]; typename T::meta_type v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; typename T::meta_type d[3]; for(ip = 0; ip < n; ++ip) { b[ip] = d[ip] = R(ip, ip); z[ip] = 0; } nrot = 0; for(i = 0; i < 50; ++i) { sm = 0; for(ip = 0; ip < n; ++ip) for(iq = ip + 1; iq < n; ++iq) sm += std::abs(R(ip, iq)); if(sm == 0.0) { vout[0].setValue(v[0][0], v[0][1], v[0][2]); vout[1].setValue(v[1][0], v[1][1], v[1][2]); vout[2].setValue(v[2][0], v[2][1], v[2][2]); dout[0] = d[0]; dout[1] = d[1]; dout[2] = d[2]; return; } if(i < 3) tresh = 0.2 * sm / (n * n); else tresh = 0.0; for(ip = 0; ip < n; ++ip) { for(iq= ip + 1; iq < n; ++iq) { g = 100.0 * std::abs(R(ip, iq)); if(i > 3 && std::abs(d[ip]) + g == std::abs(d[ip]) && std::abs(d[iq]) + g == std::abs(d[iq])) R(ip, iq) = 0.0; else if(std::abs(R(ip, iq)) > tresh) { h = d[iq] - d[ip]; if(std::abs(h) + g == std::abs(h)) t = (R(ip, iq)) / h; else { theta = 0.5 * h / (R(ip, iq)); t = 1.0 /(std::abs(theta) + std::sqrt(1.0 + theta * theta)); if(theta < 0.0) t = -t; } c = 1.0 / std::sqrt(1 + t * t); s = t * c; tau = s / (1.0 + c); h = t * R(ip, iq); z[ip] -= h; z[iq] += h; d[ip] -= h; d[iq] += h; R(ip, iq) = 0.0; for(j = 0; j < ip; ++j) { g = R(j, ip); h = R(j, iq); R(j, ip) = g - s * (h + g * tau); R(j, iq) = h + s * (g - h * tau); } for(j = ip + 1; j < iq; ++j) { g = R(ip, j); h = R(j, iq); R(ip, j) = g - s * (h + g * tau); R(j, iq) = h + s * (g - h * tau); } for(j = iq + 1; j < n; ++j) { g = R(ip, j); h = R(iq, j); R(ip, j) = g - s * (h + g * tau); R(iq, j) = h + s * (g - h * tau); } for(j = 0; j < n; ++j) { g = v[j][ip]; h = v[j][iq]; v[j][ip] = g - s * (h + g * tau); v[j][iq] = h + s * (g - h * tau); } nrot++; } } } for(ip = 0; ip < n; ++ip) { b[ip] += z[ip]; d[ip] = b[ip]; z[ip] = 0.0; } } std::cerr << "eigen: too many iterations in Jacobi transform." << std::endl; return; } template Matrix3fX abs(const Matrix3fX& R) { return Matrix3fX(abs(R.data)); } template Matrix3fX transpose(const Matrix3fX& R) { return Matrix3fX(transpose(R.data)); } template Matrix3fX inverse(const Matrix3fX& R) { return Matrix3fX(inverse(R.data)); } template typename T::meta_type quadraticForm(const Matrix3fX& R, const Vec3fX& v) { return v.dot(R * v); } #if FCL_HAVE_SSE typedef Matrix3fX Matrix3f; #else typedef Matrix3fX > Matrix3f; #endif static inline std::ostream& operator << (std::ostream& o, const Matrix3f& m) { o << "[(" << m(0, 0) << " " << m(0, 1) << " " << m(0, 2) << ")(" << m(1, 0) << " " << m(1, 1) << " " << m(1, 2) << ")(" << m(2, 0) << " " << m(2, 1) << " " << m(2, 2) << ")]"; return o; } /// @brief Class for variance matrix in 3d class Variance3f { public: /// @brief Variation matrix Matrix3f Sigma; /// @brief Variations along the eign axes Matrix3f::U sigma[3]; /// @brief Eigen axes of the variation matrix Vec3f axis[3]; Variance3f() {} Variance3f(const Matrix3f& S) : Sigma(S) { init(); } /// @brief init the Variance void init() { eigen(Sigma, sigma, axis); } /// @brief Compute the sqrt of Sigma matrix based on the eigen decomposition result, this is useful when the uncertainty matrix is initialized as a square variation matrix Variance3f& sqrt() { for(std::size_t i = 0; i < 3; ++i) { if(sigma[i] < 0) sigma[i] = 0; sigma[i] = std::sqrt(sigma[i]); } Sigma.setZero(); for(std::size_t i = 0; i < 3; ++i) { for(std::size_t j = 0; j < 3; ++j) { Sigma(i, j) += sigma[0] * axis[0][i] * axis[0][j]; Sigma(i, j) += sigma[1] * axis[1][i] * axis[1][j]; Sigma(i, j) += sigma[2] * axis[2][i] * axis[2][j]; } } return *this; } }; } #endif fcl-0.5.0/include/fcl/math/sampling.h000066400000000000000000000270711274356571000174040ustar00rootroot00000000000000#ifndef FCL_MATH_SAMPLING_H #define FCL_MATH_SAMPLING_H #include #include #include "fcl/math/constants.h" #include "fcl/math/vec_nf.h" #include "fcl/math/transform.h" namespace fcl { /// @brief Random number generation. An instance of this class /// cannot be used by multiple threads at once (member functions /// are not const). However, the constructor is thread safe and /// different instances can be used safely in any number of /// threads. It is also guaranteed that all created instances will /// have a different random seed. class RNG { public: /// @brief Constructor. Always sets a different random seed RNG(); /// @brief Generate a random real between 0 and 1 double uniform01() { return uniDist_(generator_); } /// @brief Generate a random real within given bounds: [\e lower_bound, \e upper_bound) double uniformReal(double lower_bound, double upper_bound) { assert(lower_bound <= upper_bound); return (upper_bound - lower_bound) * uniDist_(generator_) + lower_bound; } /// @brief Generate a random integer within given bounds: [\e lower_bound, \e upper_bound] int uniformInt(int lower_bound, int upper_bound) { int r = (int)floor(uniformReal((double)lower_bound, (double)(upper_bound) + 1.0)); return (r > upper_bound) ? upper_bound : r; } /// @brief Generate a random boolean bool uniformBool() { return uniDist_(generator_) <= 0.5; } /// @brief Generate a random real using a normal distribution with mean 0 and variance 1 double gaussian01() { return normalDist_(generator_); } /// @brief Generate a random real using a normal distribution with given mean and variance double gaussian(double mean, double stddev) { return normalDist_(generator_) * stddev + mean; } /// @brief Generate a random real using a half-normal distribution. The value is within specified bounds [\e r_min, \e r_max], but with a bias towards \e r_max. The function is implemended using a Gaussian distribution with mean at \e r_max - \e r_min. The distribution is 'folded' around \e r_max axis towards \e r_min. The variance of the distribution is (\e r_max - \e r_min) / \e focus. The higher the focus, the more probable it is that generated numbers are close to \e r_max. double halfNormalReal(double r_min, double r_max, double focus = 3.0); /// @brief Generate a random integer using a half-normal distribution. The value is within specified bounds ([\e r_min, \e r_max]), but with a bias towards \e r_max. The function is implemented on top of halfNormalReal() int halfNormalInt(int r_min, int r_max, double focus = 3.0); /// @brief Uniform random unit quaternion sampling. The computed value has the order (x,y,z,w) void quaternion(double value[4]); /// @brief Uniform random sampling of Euler roll-pitch-yaw angles, each in the range [-pi, pi). The computed value has the order (roll, pitch, yaw) */ void eulerRPY(double value[3]); /// @brief Uniform random sample on a disk with radius from r_min to r_max void disk(double r_min, double r_max, double& x, double& y); /// @brief Uniform random sample in a ball with radius from r_min to r_max void ball(double r_min, double r_max, double& x, double& y, double& z); /// @brief Set the seed for random number generation. Use this function to ensure the same sequence of random numbers is generated. static void setSeed(std::uint_fast32_t seed); /// @brief Get the seed used for random number generation. Passing the returned value to setSeed() at a subsequent execution of the code will ensure deterministic (repeatable) behaviour. Useful for debugging. static std::uint_fast32_t getSeed(); private: std::mt19937 generator_; std::uniform_real_distribution<> uniDist_; std::normal_distribution<> normalDist_; }; class SamplerBase { public: mutable RNG rng; }; template class SamplerR : public SamplerBase { public: SamplerR() {} SamplerR(const Vecnf& lower_bound_, const Vecnf& upper_bound_) : lower_bound(lower_bound_), upper_bound(upper_bound_) { } void setBound(const Vecnf& lower_bound_, const Vecnf& upper_bound_) { lower_bound = lower_bound_; upper_bound = upper_bound_; } void getBound(Vecnf& lower_bound_, Vecnf& upper_bound_) const { lower_bound_ = lower_bound; upper_bound_ = upper_bound; } Vecnf sample() const { Vecnf q; for(std::size_t i = 0; i < N; ++i) { q[i] = rng.uniformReal(lower_bound[i], upper_bound[i]); } return q; } private: Vecnf lower_bound; Vecnf upper_bound; }; class SamplerSE2 : public SamplerBase { public: SamplerSE2() {} SamplerSE2(const Vecnf<2>& lower_bound_, const Vecnf<2>& upper_bound_) : lower_bound(lower_bound_), upper_bound(upper_bound_) {} SamplerSE2(FCL_REAL x_min, FCL_REAL x_max, FCL_REAL y_min, FCL_REAL y_max) : lower_bound(std::vector({x_min, y_min})), upper_bound(std::vector({x_max, y_max})) {} void setBound(const Vecnf<2>& lower_bound_, const Vecnf<2>& upper_bound_) { lower_bound = lower_bound_; upper_bound = upper_bound_; } void getBound(Vecnf<2>& lower_bound_, Vecnf<2>& upper_bound_) const { lower_bound_ = lower_bound; upper_bound_ = upper_bound; } Vecnf<3> sample() const { Vecnf<3> q; q[0] = rng.uniformReal(lower_bound[0], lower_bound[1]); q[1] = rng.uniformReal(lower_bound[1], lower_bound[2]); q[2] = rng.uniformReal(-constants::pi, constants::pi); return q; } protected: Vecnf<2> lower_bound; Vecnf<2> upper_bound; }; class SamplerSE2_disk : public SamplerBase { public: SamplerSE2_disk() {} SamplerSE2_disk(FCL_REAL cx, FCL_REAL cy, FCL_REAL r1, FCL_REAL r2, FCL_REAL crefx, FCL_REAL crefy) { setBound(cx, cy, r1, r2, crefx, crefy); } void setBound(FCL_REAL cx, FCL_REAL cy, FCL_REAL r1, FCL_REAL r2, FCL_REAL crefx, FCL_REAL crefy) { c[0] = cx; c[1] = cy; cref[0] = crefx; cref[1] = crefy; r_min = r1; r_max = r2; } Vecnf<3> sample() const { Vecnf<3> q; FCL_REAL x, y; rng.disk(r_min, r_max, x, y); q[0] = x + c[0] - cref[0]; q[1] = y + c[1] - cref[1]; q[2] = rng.uniformReal(-constants::pi, constants::pi); return q; } protected: FCL_REAL c[2]; FCL_REAL cref[2]; FCL_REAL r_min, r_max; }; class SamplerSE3Euler : public SamplerBase { public: SamplerSE3Euler() {} SamplerSE3Euler(const Vecnf<3>& lower_bound_, const Vecnf<3>& upper_bound_) : lower_bound(lower_bound_), upper_bound(upper_bound_) {} SamplerSE3Euler(const Vec3f& lower_bound_, const Vec3f& upper_bound_) : lower_bound(std::vector({lower_bound_[0], lower_bound_[1], lower_bound_[2]})), upper_bound(std::vector({upper_bound_[0], upper_bound_[1], upper_bound_[2]})) {} void setBound(const Vecnf<3>& lower_bound_, const Vecnf<3>& upper_bound_) { lower_bound = lower_bound_; upper_bound = upper_bound_; } void setBound(const Vec3f& lower_bound_, const Vec3f& upper_bound_) { lower_bound = Vecnf<3>(std::vector({lower_bound_[0], lower_bound_[1], lower_bound_[2]})); upper_bound = Vecnf<3>(std::vector({upper_bound_[0], upper_bound_[1], upper_bound_[2]})); } void getBound(Vecnf<3>& lower_bound_, Vecnf<3>& upper_bound_) const { lower_bound_ = lower_bound; upper_bound_ = upper_bound; } void getBound(Vec3f& lower_bound_, Vec3f& upper_bound_) const { lower_bound_ = Vec3f(lower_bound[0], lower_bound[1], lower_bound[2]); upper_bound_ = Vec3f(upper_bound[0], upper_bound[1], upper_bound[2]); } Vecnf<6> sample() const { Vecnf<6> q; q[0] = rng.uniformReal(lower_bound[0], upper_bound[0]); q[1] = rng.uniformReal(lower_bound[1], upper_bound[1]); q[2] = rng.uniformReal(lower_bound[2], upper_bound[2]); FCL_REAL s[4]; rng.quaternion(s); Quaternion3f quat(s[0], s[1], s[2], s[3]); FCL_REAL a, b, c; quat.toEuler(a, b, c); q[3] = a; q[4] = b; q[5] = c; return q; } protected: Vecnf<3> lower_bound; Vecnf<3> upper_bound; }; class SamplerSE3Quat : public SamplerBase { public: SamplerSE3Quat() {} SamplerSE3Quat(const Vecnf<3>& lower_bound_, const Vecnf<3>& upper_bound_) : lower_bound(lower_bound_), upper_bound(upper_bound_) {} SamplerSE3Quat(const Vec3f& lower_bound_, const Vec3f& upper_bound_) : lower_bound(std::vector({lower_bound_[0], lower_bound_[1], lower_bound_[2]})), upper_bound(std::vector({upper_bound_[0], upper_bound_[1], upper_bound_[2]})) {} void setBound(const Vecnf<3>& lower_bound_, const Vecnf<3>& upper_bound_) { lower_bound = lower_bound_; upper_bound = upper_bound_; } void setBound(const Vec3f& lower_bound_, const Vec3f& upper_bound_) { lower_bound = Vecnf<3>(std::vector({lower_bound_[0], lower_bound_[1], lower_bound_[2]})); upper_bound = Vecnf<3>(std::vector({upper_bound_[0], upper_bound_[1], upper_bound_[2]})); } void getBound(Vecnf<3>& lower_bound_, Vecnf<3>& upper_bound_) const { lower_bound_ = lower_bound; upper_bound_ = upper_bound; } void getBound(Vec3f& lower_bound_, Vec3f& upper_bound_) const { lower_bound_ = Vec3f(lower_bound[0], lower_bound[1], lower_bound[2]); upper_bound_ = Vec3f(upper_bound[0], upper_bound[1], upper_bound[2]); } Vecnf<7> sample() const { Vecnf<7> q; q[0] = rng.uniformReal(lower_bound[0], upper_bound[0]); q[1] = rng.uniformReal(lower_bound[1], upper_bound[1]); q[2] = rng.uniformReal(lower_bound[2], upper_bound[2]); FCL_REAL s[4]; rng.quaternion(s); q[3] = s[0]; q[4] = s[1]; q[5] = s[2]; q[6] = s[3]; return q; } protected: Vecnf<3> lower_bound; Vecnf<3> upper_bound; }; class SamplerSE3Euler_ball : public SamplerBase { public: SamplerSE3Euler_ball() {} SamplerSE3Euler_ball(FCL_REAL r_) : r(r_) { } void setBound(const FCL_REAL& r_) { r = r_; } void getBound(FCL_REAL& r_) const { r_ = r; } Vecnf<6> sample() const { Vecnf<6> q; FCL_REAL x, y, z; rng.ball(0, r, x, y, z); q[0] = x; q[1] = y; q[2] = z; FCL_REAL s[4]; rng.quaternion(s); Quaternion3f quat(s[0], s[1], s[2], s[3]); FCL_REAL a, b, c; quat.toEuler(a, b, c); q[3] = a; q[4] = b; q[5] = c; return q; } protected: FCL_REAL r; }; class SamplerSE3Quat_ball : public SamplerBase { public: SamplerSE3Quat_ball() {} SamplerSE3Quat_ball(FCL_REAL r_) : r(r_) {} void setBound(const FCL_REAL& r_) { r = r_; } void getBound(FCL_REAL& r_) const { r_ = r; } Vecnf<7> sample() const { Vecnf<7> q; FCL_REAL x, y, z; rng.ball(0, r, x, y, z); q[0] = x; q[1] = y; q[2] = z; FCL_REAL s[4]; rng.quaternion(s); q[3] = s[0]; q[4] = s[1]; q[5] = s[2]; q[6] = s[3]; return q; } protected: FCL_REAL r; }; } #endif fcl-0.5.0/include/fcl/math/transform.h000066400000000000000000000242511274356571000176020ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_TRANSFORM_H #define FCL_TRANSFORM_H #include "fcl/math/matrix_3f.h" #include namespace fcl { /// @brief Quaternion used locally by InterpMotion class Quaternion3f { public: /// @brief Default quaternion is identity rotation Quaternion3f() { data[0] = 1; data[1] = 0; data[2] = 0; data[3] = 0; } Quaternion3f(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d) { data[0] = a; data[1] = b; data[2] = c; data[3] = d; } /// @brief Whether the rotation is identity bool isIdentity() const { return (data[0] == 1) && (data[1] == 0) && (data[2] == 0) && (data[3] == 0); } /// @brief Matrix to quaternion void fromRotation(const Matrix3f& R); /// @brief Quaternion to matrix void toRotation(Matrix3f& R) const; /// @brief Euler to quaternion void fromEuler(FCL_REAL a, FCL_REAL b, FCL_REAL c); /// @brief Quaternion to Euler void toEuler(FCL_REAL& a, FCL_REAL& b, FCL_REAL& c) const; /// @brief Axes to quaternion void fromAxes(const Vec3f axis[3]); /// @brief Axes to matrix void toAxes(Vec3f axis[3]) const; /// @brief Axis and angle to quaternion void fromAxisAngle(const Vec3f& axis, FCL_REAL angle); /// @brief Quaternion to axis and angle void toAxisAngle(Vec3f& axis, FCL_REAL& angle) const; /// @brief Dot product between quaternions FCL_REAL dot(const Quaternion3f& other) const; /// @brief addition Quaternion3f operator + (const Quaternion3f& other) const; const Quaternion3f& operator += (const Quaternion3f& other); /// @brief minus Quaternion3f operator - (const Quaternion3f& other) const; const Quaternion3f& operator -= (const Quaternion3f& other); /// @brief multiplication Quaternion3f operator * (const Quaternion3f& other) const; const Quaternion3f& operator *= (const Quaternion3f& other); /// @brief division Quaternion3f operator - () const; /// @brief scalar multiplication Quaternion3f operator * (FCL_REAL t) const; const Quaternion3f& operator *= (FCL_REAL t); /// @brief conjugate Quaternion3f& conj(); /// @brief inverse Quaternion3f& inverse(); /// @brief rotate a vector Vec3f transform(const Vec3f& v) const; inline const FCL_REAL& getW() const { return data[0]; } inline const FCL_REAL& getX() const { return data[1]; } inline const FCL_REAL& getY() const { return data[2]; } inline const FCL_REAL& getZ() const { return data[3]; } inline FCL_REAL& getW() { return data[0]; } inline FCL_REAL& getX() { return data[1]; } inline FCL_REAL& getY() { return data[2]; } inline FCL_REAL& getZ() { return data[3]; } Vec3f getColumn(std::size_t i) const; Vec3f getRow(std::size_t i) const; bool operator == (const Quaternion3f& other) const { for(std::size_t i = 0; i < 4; ++i) { if(data[i] != other[i]) return false; } return true; } bool operator != (const Quaternion3f& other) const { return !(*this == other); } FCL_REAL operator [] (std::size_t i) const { return data[i]; } private: FCL_REAL data[4]; }; /// @brief conjugate of quaternion Quaternion3f conj(const Quaternion3f& q); /// @brief inverse of quaternion Quaternion3f inverse(const Quaternion3f& q); static inline std::ostream& operator << (std::ostream& o, const Quaternion3f& q) { o << "(" << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << ")"; return o; } /// @brief Simple transform class used locally by InterpMotion class Transform3f { std::mutex lock_; /// @brief Whether matrix cache is set mutable bool matrix_set; /// @brief Matrix cache mutable Matrix3f R; /// @brief Tranlation vector Vec3f T; /// @brief Rotation Quaternion3f q; const Matrix3f& getRotationInternal() const; public: /// @brief Default transform is no movement Transform3f() { setIdentity(); // set matrix_set true } /// @brief Construct transform from rotation and translation Transform3f(const Matrix3f& R_, const Vec3f& T_) : matrix_set(true), R(R_), T(T_) { q.fromRotation(R_); } /// @brief Construct transform from rotation and translation Transform3f(const Quaternion3f& q_, const Vec3f& T_) : matrix_set(false), T(T_), q(q_) { } /// @brief Construct transform from rotation Transform3f(const Matrix3f& R_) : matrix_set(true), R(R_) { q.fromRotation(R_); } /// @brief Construct transform from rotation Transform3f(const Quaternion3f& q_) : matrix_set(false), q(q_) { } /// @brief Construct transform from translation Transform3f(const Vec3f& T_) : matrix_set(true), T(T_) { R.setIdentity(); } /// @brief Construct transform from another transform Transform3f(const Transform3f& tf) : matrix_set(tf.matrix_set), R(tf.R), T(tf.T), q(tf.q) { } /// @brief operator = Transform3f& operator = (const Transform3f& tf) { matrix_set = tf.matrix_set; R = tf.R; q = tf.q; T = tf.T; return *this; } /// @brief get translation inline const Vec3f& getTranslation() const { return T; } /// @brief get rotation inline const Matrix3f& getRotation() const { if(matrix_set) return R; return getRotationInternal(); } /// @brief get quaternion inline const Quaternion3f& getQuatRotation() const { return q; } /// @brief set transform from rotation and translation inline void setTransform(const Matrix3f& R_, const Vec3f& T_) { R = R_; T = T_; matrix_set = true; q.fromRotation(R_); } /// @brief set transform from rotation and translation inline void setTransform(const Quaternion3f& q_, const Vec3f& T_) { matrix_set = false; q = q_; T = T_; } /// @brief set transform from rotation inline void setRotation(const Matrix3f& R_) { R = R_; matrix_set = true; q.fromRotation(R_); } /// @brief set transform from translation inline void setTranslation(const Vec3f& T_) { T = T_; } /// @brief set transform from rotation inline void setQuatRotation(const Quaternion3f& q_) { matrix_set = false; q = q_; } /// @brief transform a given vector by the transform inline Vec3f transform(const Vec3f& v) const { return q.transform(v) + T; } /// @brief inverse transform inline Transform3f& inverse() { matrix_set = false; q.conj(); T = q.transform(-T); return *this; } /// @brief inverse the transform and multiply with another inline Transform3f inverseTimes(const Transform3f& other) const { const Quaternion3f& q_inv = fcl::conj(q); return Transform3f(q_inv * other.q, q_inv.transform(other.T - T)); } /// @brief multiply with another transform inline const Transform3f& operator *= (const Transform3f& other) { matrix_set = false; T = q.transform(other.T) + T; q *= other.q; return *this; } /// @brief multiply with another transform inline Transform3f operator * (const Transform3f& other) const { Quaternion3f q_new = q * other.q; return Transform3f(q_new, q.transform(other.T) + T); } /// @brief check whether the transform is identity inline bool isIdentity() const { return q.isIdentity() && T.isZero(); } /// @brief set the transform to be identity transform inline void setIdentity() { R.setIdentity(); T.setValue(0); q = Quaternion3f(); matrix_set = true; } bool operator == (const Transform3f& other) const { return (q == other.getQuatRotation()) && (T == other.getTranslation()); } bool operator != (const Transform3f& other) const { return !(*this == other); } }; /// @brief inverse the transform Transform3f inverse(const Transform3f& tf); /// @brief compute the relative transform between two transforms: tf2 = tf1 * tf (relative to the local coordinate system in tf1) void relativeTransform(const Transform3f& tf1, const Transform3f& tf2, Transform3f& tf); /// @brief compute the relative transform between two transforms: tf2 = tf * tf1 (relative to the global coordinate system) void relativeTransform2(const Transform3f& tf1, const Transform3f& tf2, Transform3f& tf); } #endif fcl-0.5.0/include/fcl/math/vec_3f.h000066400000000000000000000173461274356571000167430ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_VEC_3F_H #define FCL_VEC_3F_H #include "fcl/config.h" #include "fcl/data_types.h" #include "fcl/math/math_details.h" #if FCL_HAVE_SSE # include "fcl/simd/math_simd_details.h" #endif #include #include #include namespace fcl { /// @brief Vector3 class wrapper. The core data is in the template parameter class. template class Vec3fX { public: typedef typename T::meta_type U; /// @brief interval vector3 data T data; Vec3fX() {} Vec3fX(const Vec3fX& other) : data(other.data) {} /// @brief create Vector (x, y, z) Vec3fX(U x, U y, U z) : data(x, y, z) {} /// @brief create vector (x, x, x) Vec3fX(U x) : data(x) {} /// @brief create vector using the internal data type Vec3fX(const T& data_) : data(data_) {} inline U operator [] (size_t i) const { return data[i]; } inline U& operator [] (size_t i) { return data[i]; } inline Vec3fX operator + (const Vec3fX& other) const { return Vec3fX(data + other.data); } inline Vec3fX operator - (const Vec3fX& other) const { return Vec3fX(data - other.data); } inline Vec3fX operator * (const Vec3fX& other) const { return Vec3fX(data * other.data); } inline Vec3fX operator / (const Vec3fX& other) const { return Vec3fX(data / other.data); } inline Vec3fX& operator += (const Vec3fX& other) { data += other.data; return *this; } inline Vec3fX& operator -= (const Vec3fX& other) { data -= other.data; return *this; } inline Vec3fX& operator *= (const Vec3fX& other) { data *= other.data; return *this; } inline Vec3fX& operator /= (const Vec3fX& other) { data /= other.data; return *this; } inline Vec3fX operator + (U t) const { return Vec3fX(data + t); } inline Vec3fX operator - (U t) const { return Vec3fX(data - t); } inline Vec3fX operator * (U t) const { return Vec3fX(data * t); } inline Vec3fX operator / (U t) const { return Vec3fX(data / t); } inline Vec3fX& operator += (U t) { data += t; return *this; } inline Vec3fX& operator -= (U t) { data -= t; return *this; } inline Vec3fX& operator *= (U t) { data *= t; return *this; } inline Vec3fX& operator /= (U t) { data /= t; return *this; } inline Vec3fX operator - () const { return Vec3fX(-data); } inline Vec3fX cross(const Vec3fX& other) const { return Vec3fX(details::cross_prod(data, other.data)); } inline U dot(const Vec3fX& other) const { return details::dot_prod3(data, other.data); } inline Vec3fX& normalize() { U sqr_length = details::dot_prod3(data, data); if(sqr_length > 0) *this /= (U)sqrt(sqr_length); return *this; } inline Vec3fX& normalize(bool* signal) { U sqr_length = details::dot_prod3(data, data); if(sqr_length > 0) { *this /= (U)sqrt(sqr_length); *signal = true; } else *signal = false; return *this; } inline Vec3fX& abs() { data = abs(data); return *this; } inline U length() const { return sqrt(details::dot_prod3(data, data)); } inline U norm() const { return sqrt(details::dot_prod3(data, data)); } inline U sqrLength() const { return details::dot_prod3(data, data); } inline U squaredNorm() const { return details::dot_prod3(data, data); } inline void setValue(U x, U y, U z) { data.setValue(x, y, z); } inline void setValue(U x) { data.setValue(x); } inline void setZero () {data.setValue (0); } inline bool equal(const Vec3fX& other, U epsilon = std::numeric_limits::epsilon() * 100) const { return details::equal(data, other.data, epsilon); } inline Vec3fX& negate() { data.negate(); return *this; } bool operator == (const Vec3fX& other) const { return equal(other, 0); } bool operator != (const Vec3fX& other) const { return !(*this == other); } inline Vec3fX& ubound(const Vec3fX& u) { data.ubound(u.data); return *this; } inline Vec3fX& lbound(const Vec3fX& l) { data.lbound(l.data); return *this; } bool isZero() const { return (data[0] == 0) && (data[1] == 0) && (data[2] == 0); } }; template static inline Vec3fX normalize(const Vec3fX& v) { typename T::meta_type sqr_length = details::dot_prod3(v.data, v.data); if(sqr_length > 0) return v / (typename T::meta_type)sqrt(sqr_length); else return v; } template static inline typename T::meta_type triple(const Vec3fX& x, const Vec3fX& y, const Vec3fX& z) { return x.dot(y.cross(z)); } template std::ostream& operator << (std::ostream& out, const Vec3fX& x) { out << x[0] << " " << x[1] << " " << x[2]; return out; } template static inline Vec3fX min(const Vec3fX& x, const Vec3fX& y) { return Vec3fX(details::min(x.data, y.data)); } template static inline Vec3fX max(const Vec3fX& x, const Vec3fX& y) { return Vec3fX(details::max(x.data, y.data)); } template static inline Vec3fX abs(const Vec3fX& x) { return Vec3fX(details::abs(x.data)); } template void generateCoordinateSystem(const Vec3fX& w, Vec3fX& u, Vec3fX& v) { typedef typename T::meta_type U; U inv_length; if(std::abs(w[0]) >= std::abs(w[1])) { inv_length = (U)1.0 / sqrt(w[0] * w[0] + w[2] * w[2]); u[0] = -w[2] * inv_length; u[1] = (U)0; u[2] = w[0] * inv_length; v[0] = w[1] * u[2]; v[1] = w[2] * u[0] - w[0] * u[2]; v[2] = -w[1] * u[0]; } else { inv_length = (U)1.0 / sqrt(w[1] * w[1] + w[2] * w[2]); u[0] = (U)0; u[1] = w[2] * inv_length; u[2] = -w[1] * inv_length; v[0] = w[1] * u[2] - w[2] * u[1]; v[1] = -w[0] * u[2]; v[2] = w[0] * u[1]; } } #if FCL_HAVE_SSE typedef Vec3fX Vec3f; #else typedef Vec3fX > Vec3f; #endif static inline std::ostream& operator << (std::ostream& o, const Vec3f& v) { o << "(" << v[0] << " " << v[1] << " " << v[2] << ")"; return o; } template inline Vec3fX operator * (const typename Vec3fX ::U& t, const Vec3fX & v) { return Vec3fX (v.data * t); } } #endif fcl-0.5.0/include/fcl/math/vec_nf.h000066400000000000000000000101561274356571000170260ustar00rootroot00000000000000#ifndef FCL_MATH_VEC_NF_H #define FCL_MATH_VEC_NF_H #include #include #include #include #include #include "fcl/data_types.h" namespace fcl { template class Vec_n { public: Vec_n() { data.resize(N, 0); } template Vec_n(const Vec_n& data_) { std::size_t n = std::min(M, N); for(std::size_t i = 0; i < n; ++i) data[i] = data_[i]; } Vec_n(const std::vector& data_) { data.resize(N, 0); std::size_t n = std::min(data_.size(), N); for(std::size_t i = 0; i < n; ++i) data[i] = data_[i]; } bool operator == (const Vec_n& other) const { for(std::size_t i = 0; i < N; ++i) { if(data[i] != other[i]) return false; } return true; } bool operator != (const Vec_n& other) const { for(std::size_t i = 0; i < N; ++i) { if(data[i] != other[i]) return true; } return false; } std::size_t dim() const { return N; } void setData(const std::vector& data_) { std::size_t n = std::min(data.size(), N); for(std::size_t i = 0; i < n; ++i) data[i] = data_[i]; } T operator [] (std::size_t i) const { return data[i]; } T& operator [] (std::size_t i) { return data[i]; } Vec_n operator + (const Vec_n& other) const { Vec_n result; for(std::size_t i = 0; i < N; ++i) result[i] = data[i] + other[i]; return result; } Vec_n& operator += (const Vec_n& other) { for(std::size_t i = 0; i < N; ++i) data[i] += other[i]; return *this; } Vec_n operator - (const Vec_n& other) const { Vec_n result; for(std::size_t i = 0; i < N; ++i) result[i] = data[i] - other[i]; return result; } Vec_n& operator -= (const Vec_n& other) { for(std::size_t i = 0; i < N; ++i) data[i] -= other[i]; return *this; } Vec_n operator * (T t) const { Vec_n result; for(std::size_t i = 0; i < N; ++i) result[i] = data[i] * t; return result; } Vec_n& operator *= (T t) { for(std::size_t i = 0; i < N; ++i) data[i] *= t; return *this; } Vec_n operator / (T t) const { Vec_n result; for(std::size_t i = 0; i < N; ++i) result[i] = data[i] / 5; return result; } Vec_n& operator /= (T t) { for(std::size_t i = 0; i < N; ++i) data[i] /= t; return *this; } Vec_n& setZero() { for(std::size_t i = 0; i < N; ++i) data[i] = 0; } T dot(const Vec_n& other) const { T sum = 0; for(std::size_t i = 0; i < N; ++i) sum += data[i] * other[i]; return sum; } std::vector getData() const { return data; } protected: std::vector data; }; template void repack(const Vec_n& input, Vec_n& output) { std::size_t n = std::min(N1, N2); for(std::size_t i = 0; i < n; ++i) output[i] = input[i]; } template Vec_n operator * (T t, const Vec_n& v) { return v * t; } template Vec_n combine(const Vec_n& v1, const Vec_n& v2) { Vec_n v; for(std::size_t i = 0; i < N; ++i) v[i] = v1[i]; for(std::size_t i = 0; i < M; ++i) v[i + N] = v2[i]; return v; } template std::ostream& operator << (std::ostream& o, const Vec_n& v) { o << "("; for(std::size_t i = 0; i < N; ++i) { if(i == N - 1) o << v[i] << ")"; else o << v[i] << " "; } return o; } // workaround for template alias template class Vecnf : public Vec_n { public: Vecnf(const Vec_n& other) : Vec_n(other) {} Vecnf() : Vec_n() {} template Vecnf(const Vec_n& other) : Vec_n(other) {} Vecnf(const std::vector& data_) : Vec_n(data_) {} }; } #endif fcl-0.5.0/include/fcl/narrowphase/000077500000000000000000000000001274356571000170125ustar00rootroot00000000000000fcl-0.5.0/include/fcl/narrowphase/gjk.h000066400000000000000000000207621274356571000177450ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_GJK_H #define FCL_GJK_H #include "fcl/shape/geometric_shapes.h" #include "fcl/math/transform.h" namespace fcl { namespace details { /// @brief the support function for shape Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir); /// @brief Minkowski difference class of two shapes struct MinkowskiDiff { /// @brief points to two shapes const ShapeBase* shapes[2]; /// @brief rotation from shape0 to shape1 Matrix3f toshape1; /// @brief transform from shape1 to shape0 Transform3f toshape0; MinkowskiDiff() { } /// @brief support function for shape0 inline Vec3f support0(const Vec3f& d) const { return getSupport(shapes[0], d); } /// @brief support function for shape1 inline Vec3f support1(const Vec3f& d) const { return toshape0.transform(getSupport(shapes[1], toshape1 * d)); } /// @brief support function for the pair of shapes inline Vec3f support(const Vec3f& d) const { return support0(d) - support1(-d); } /// @brief support function for the d-th shape (d = 0 or 1) inline Vec3f support(const Vec3f& d, size_t index) const { if(index) return support1(d); else return support0(d); } /// @brief support function for translating shape0, which is translating at velocity v inline Vec3f support0(const Vec3f& d, const Vec3f& v) const { if(d.dot(v) <= 0) return getSupport(shapes[0], d); else return getSupport(shapes[0], d) + v; } /// @brief support function for the pair of shapes, where shape0 is translating at velocity v inline Vec3f support(const Vec3f& d, const Vec3f& v) const { return support0(d, v) - support1(-d); } /// @brief support function for the d-th shape (d = 0 or 1), where shape0 is translating at velocity v inline Vec3f support(const Vec3f& d, const Vec3f& v, size_t index) const { if(index) return support1(d); else return support0(d, v); } }; /// @brief class for GJK algorithm struct GJK { struct SimplexV { /// @brief support direction Vec3f d; /// @brieg support vector (i.e., the furthest point on the shape along the support direction) Vec3f w; }; struct Simplex { /// @brief simplex vertex SimplexV* c[4]; /// @brief weight FCL_REAL p[4]; /// @brief size of simplex (number of vertices) size_t rank; Simplex() : rank(0) {} }; enum Status {Valid, Inside, Failed}; MinkowskiDiff shape; Vec3f ray; FCL_REAL distance; Simplex simplices[2]; GJK(unsigned int max_iterations_, FCL_REAL tolerance_) : max_iterations(max_iterations_), tolerance(tolerance_) { initialize(); } void initialize(); /// @brief GJK algorithm, given the initial value guess Status evaluate(const MinkowskiDiff& shape_, const Vec3f& guess); /// @brief apply the support function along a direction, the result is return in sv void getSupport(const Vec3f& d, SimplexV& sv) const; /// @brief apply the support function along a direction, the result is return is sv, here shape0 is translating at velocity v void getSupport(const Vec3f& d, const Vec3f& v, SimplexV& sv) const; /// @brief discard one vertex from the simplex void removeVertex(Simplex& simplex); /// @brief append one vertex to the simplex void appendVertex(Simplex& simplex, const Vec3f& v); /// @brief whether the simplex enclose the origin bool encloseOrigin(); /// @brief get the underlying simplex using in GJK, can be used for cache in next iteration inline Simplex* getSimplex() const { return simplex; } /// @brief get the guess from current simplex Vec3f getGuessFromSimplex() const; private: SimplexV store_v[4]; SimplexV* free_v[4]; size_t nfree; size_t current; Simplex* simplex; Status status; unsigned int max_iterations; FCL_REAL tolerance; }; static const size_t EPA_MAX_FACES = 128; static const size_t EPA_MAX_VERTICES = 64; static const FCL_REAL EPA_EPS = 0.000001; static const size_t EPA_MAX_ITERATIONS = 255; /// @brief class for EPA algorithm struct EPA { private: typedef GJK::SimplexV SimplexV; struct SimplexF { Vec3f n; FCL_REAL d; SimplexV* c[3]; // a face has three vertices SimplexF* f[3]; // a face has three adjacent faces SimplexF* l[2]; // the pre and post faces in the list size_t e[3]; size_t pass; }; struct SimplexList { SimplexF* root; size_t count; SimplexList() : root(NULL), count(0) {} void append(SimplexF* face) { face->l[0] = NULL; face->l[1] = root; if(root) root->l[0] = face; root = face; ++count; } void remove(SimplexF* face) { if(face->l[1]) face->l[1]->l[0] = face->l[0]; if(face->l[0]) face->l[0]->l[1] = face->l[1]; if(face == root) root = face->l[1]; --count; } }; static inline void bind(SimplexF* fa, size_t ea, SimplexF* fb, size_t eb) { fa->e[ea] = eb; fa->f[ea] = fb; fb->e[eb] = ea; fb->f[eb] = fa; } struct SimplexHorizon { SimplexF* cf; // current face in the horizon SimplexF* ff; // first face in the horizon size_t nf; // number of faces in the horizon SimplexHorizon() : cf(NULL), ff(NULL), nf(0) {} }; private: unsigned int max_face_num; unsigned int max_vertex_num; unsigned int max_iterations; FCL_REAL tolerance; public: enum Status {Valid, Touching, Degenerated, NonConvex, InvalidHull, OutOfFaces, OutOfVertices, AccuracyReached, FallBack, Failed}; Status status; GJK::Simplex result; Vec3f normal; FCL_REAL depth; SimplexV* sv_store; SimplexF* fc_store; size_t nextsv; SimplexList hull, stock; EPA(unsigned int max_face_num_, unsigned int max_vertex_num_, unsigned int max_iterations_, FCL_REAL tolerance_) : max_face_num(max_face_num_), max_vertex_num(max_vertex_num_), max_iterations(max_iterations_), tolerance(tolerance_) { initialize(); } ~EPA() { delete [] sv_store; delete [] fc_store; } void initialize(); bool getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, FCL_REAL& dist); SimplexF* newFace(SimplexV* a, SimplexV* b, SimplexV* c, bool forced); /// @brief Find the best polytope face to split SimplexF* findBest(); Status evaluate(GJK& gjk, const Vec3f& guess); /// @brief the goal is to add a face connecting vertex w and face edge f[e] bool expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon& horizon); }; } // details } #endif fcl-0.5.0/include/fcl/narrowphase/gjk_libccd.h000066400000000000000000000132401274356571000212360ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_GJK_LIBCCD_H #define FCL_GJK_LIBCCD_H #include "fcl/shape/geometric_shapes.h" #include "fcl/math/transform.h" #include #include namespace fcl { namespace details { /// @brief callback function used by GJK algorithm typedef void (*GJKSupportFunction)(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v); typedef void (*GJKCenterFunction)(const void* obj, ccd_vec3_t* c); /// @brief initialize GJK stuffs template class GJKInitializer { public: /// @brief Get GJK support function static GJKSupportFunction getSupportFunction() { return NULL; } /// @brief Get GJK center function static GJKCenterFunction getCenterFunction() { return NULL; } /// @brief Get GJK object from a shape /// Notice that only local transformation is applied. /// Gloal transformation are considered later static void* createGJKObject(const T& /* s */, const Transform3f& /*tf*/) { return NULL; } /// @brief Delete GJK object static void deleteGJKObject(void* o) {} }; /// @brief initialize GJK Cylinder template<> class GJKInitializer { public: static GJKSupportFunction getSupportFunction(); static GJKCenterFunction getCenterFunction(); static void* createGJKObject(const Cylinder& s, const Transform3f& tf); static void deleteGJKObject(void* o); }; /// @brief initialize GJK Sphere template<> class GJKInitializer { public: static GJKSupportFunction getSupportFunction(); static GJKCenterFunction getCenterFunction(); static void* createGJKObject(const Sphere& s, const Transform3f& tf); static void deleteGJKObject(void* o); }; /// @brief initialize GJK Ellipsoid template<> class GJKInitializer { public: static GJKSupportFunction getSupportFunction(); static GJKCenterFunction getCenterFunction(); static void* createGJKObject(const Ellipsoid& s, const Transform3f& tf); static void deleteGJKObject(void* o); }; /// @brief initialize GJK Box template<> class GJKInitializer { public: static GJKSupportFunction getSupportFunction(); static GJKCenterFunction getCenterFunction(); static void* createGJKObject(const Box& s, const Transform3f& tf); static void deleteGJKObject(void* o); }; /// @brief initialize GJK Capsule template<> class GJKInitializer { public: static GJKSupportFunction getSupportFunction(); static GJKCenterFunction getCenterFunction(); static void* createGJKObject(const Capsule& s, const Transform3f& tf); static void deleteGJKObject(void* o); }; /// @brief initialize GJK Cone template<> class GJKInitializer { public: static GJKSupportFunction getSupportFunction(); static GJKCenterFunction getCenterFunction(); static void* createGJKObject(const Cone& s, const Transform3f& tf); static void deleteGJKObject(void* o); }; /// @brief initialize GJK Convex template<> class GJKInitializer { public: static GJKSupportFunction getSupportFunction(); static GJKCenterFunction getCenterFunction(); static void* createGJKObject(const Convex& s, const Transform3f& tf); static void deleteGJKObject(void* o); }; /// @brief initialize GJK Triangle GJKSupportFunction triGetSupportFunction(); GJKCenterFunction triGetCenterFunction(); void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3); void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf); void triDeleteGJKObject(void* o); /// @brief GJK collision algorithm bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1, void* obj2, ccd_support_fn supp2, ccd_center_fn cen2, unsigned int max_iterations, FCL_REAL tolerance, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal); bool GJKDistance(void* obj1, ccd_support_fn supp1, void* obj2, ccd_support_fn supp2, unsigned int max_iterations, FCL_REAL tolerance, FCL_REAL* dist, Vec3f* p1, Vec3f* p2); } // details } #endif fcl-0.5.0/include/fcl/narrowphase/narrowphase.h000077500000000000000000001411211274356571000215170ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_NARROWPHASE_H #define FCL_NARROWPHASE_H #include #include "fcl/collision_data.h" #include "fcl/narrowphase/gjk.h" #include "fcl/narrowphase/gjk_libccd.h" namespace fcl { /// @brief collision and distance solver based on libccd library. struct GJKSolver_libccd { /// @brief intersection checking between two shapes /// @deprecated use shapeIntersect(const S1&, const Transform3f&, const S2&, const Transform3f&, std::vector*) const template FCL_DEPRECATED bool shapeIntersect(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; /// @brief intersection checking between two shapes template bool shapeIntersect(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2, std::vector* contacts = NULL) const { void* o1 = details::GJKInitializer::createGJKObject(s1, tf1); void* o2 = details::GJKInitializer::createGJKObject(s2, tf2); bool res; if(contacts) { Vec3f normal; Vec3f point; FCL_REAL depth; res = details::GJKCollide(o1, details::GJKInitializer::getSupportFunction(), details::GJKInitializer::getCenterFunction(), o2, details::GJKInitializer::getSupportFunction(), details::GJKInitializer::getCenterFunction(), max_collision_iterations, collision_tolerance, &point, &depth, &normal); contacts->push_back(ContactPoint(normal, point, depth)); } else { res = details::GJKCollide(o1, details::GJKInitializer::getSupportFunction(), details::GJKInitializer::getCenterFunction(), o2, details::GJKInitializer::getSupportFunction(), details::GJKInitializer::getCenterFunction(), max_collision_iterations, collision_tolerance, NULL, NULL, NULL); } details::GJKInitializer::deleteGJKObject(o1); details::GJKInitializer::deleteGJKObject(o2); return res; } /// @brief intersection checking between one shape and a triangle template bool shapeTriangleIntersect(const S& s, const Transform3f& tf, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points = NULL, FCL_REAL* penetration_depth = NULL, Vec3f* normal = NULL) const { void* o1 = details::GJKInitializer::createGJKObject(s, tf); void* o2 = details::triCreateGJKObject(P1, P2, P3); bool res = details::GJKCollide(o1, details::GJKInitializer::getSupportFunction(), details::GJKInitializer::getCenterFunction(), o2, details::triGetSupportFunction(), details::triGetCenterFunction(), max_collision_iterations, collision_tolerance, contact_points, penetration_depth, normal); details::GJKInitializer::deleteGJKObject(o1); details::triDeleteGJKObject(o2); return res; } //// @brief intersection checking between one shape and a triangle with transformation template bool shapeTriangleIntersect(const S& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points = NULL, FCL_REAL* penetration_depth = NULL, Vec3f* normal = NULL) const { void* o1 = details::GJKInitializer::createGJKObject(s, tf1); void* o2 = details::triCreateGJKObject(P1, P2, P3, tf2); bool res = details::GJKCollide(o1, details::GJKInitializer::getSupportFunction(), details::GJKInitializer::getCenterFunction(), o2, details::triGetSupportFunction(), details::triGetCenterFunction(), max_collision_iterations, collision_tolerance, contact_points, penetration_depth, normal); details::GJKInitializer::deleteGJKObject(o1); details::triDeleteGJKObject(o2); return res; } /// @brief distance computation between two shapes template bool shapeDistance(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2, FCL_REAL* dist = NULL, Vec3f* p1 = NULL, Vec3f* p2 = NULL) const { void* o1 = details::GJKInitializer::createGJKObject(s1, tf1); void* o2 = details::GJKInitializer::createGJKObject(s2, tf2); bool res = details::GJKDistance(o1, details::GJKInitializer::getSupportFunction(), o2, details::GJKInitializer::getSupportFunction(), max_distance_iterations, distance_tolerance, dist, p1, p2); if(p1) *p1 = inverse(tf1).transform(*p1); if(p2) *p2 = inverse(tf2).transform(*p2); details::GJKInitializer::deleteGJKObject(o1); details::GJKInitializer::deleteGJKObject(o2); return res; } /// @brief distance computation between one shape and a triangle template bool shapeTriangleDistance(const S& s, const Transform3f& tf, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, FCL_REAL* dist = NULL, Vec3f* p1 = NULL, Vec3f* p2 = NULL) const { void* o1 = details::GJKInitializer::createGJKObject(s, tf); void* o2 = details::triCreateGJKObject(P1, P2, P3); bool res = details::GJKDistance(o1, details::GJKInitializer::getSupportFunction(), o2, details::triGetSupportFunction(), max_distance_iterations, distance_tolerance, dist, p1, p2); if(p1) *p1 = inverse(tf).transform(*p1); details::GJKInitializer::deleteGJKObject(o1); details::triDeleteGJKObject(o2); return res; } /// @brief distance computation between one shape and a triangle with transformation template bool shapeTriangleDistance(const S& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, FCL_REAL* dist = NULL, Vec3f* p1 = NULL, Vec3f* p2 = NULL) const { void* o1 = details::GJKInitializer::createGJKObject(s, tf1); void* o2 = details::triCreateGJKObject(P1, P2, P3, tf2); bool res = details::GJKDistance(o1, details::GJKInitializer::getSupportFunction(), o2, details::triGetSupportFunction(), max_distance_iterations, distance_tolerance, dist, p1, p2); if(p1) *p1 = inverse(tf1).transform(*p1); if(p2) *p2 = inverse(tf2).transform(*p2); details::GJKInitializer::deleteGJKObject(o1); details::triDeleteGJKObject(o2); return res; } /// @brief default setting for GJK algorithm GJKSolver_libccd() { max_collision_iterations = 500; max_distance_iterations = 1000; collision_tolerance = 1e-6; distance_tolerance = 1e-6; } void enableCachedGuess(bool if_enable) const { // TODO: need change libccd to exploit spatial coherence } void setCachedGuess(const Vec3f& guess) const { // TODO: need change libccd to exploit spatial coherence } Vec3f getCachedGuess() const { return Vec3f(-1, 0, 0); } /// @brief maximum number of iterations used in GJK algorithm for collision unsigned int max_collision_iterations; /// @brief maximum number of iterations used in GJK algorithm for distance unsigned int max_distance_iterations; /// @brief the threshold used in GJK algorithm to stop collision iteration FCL_REAL collision_tolerance; /// @brief the threshold used in GJK algorithm to stop distance iteration FCL_REAL distance_tolerance; }; template bool GJKSolver_libccd::shapeIntersect(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const { bool res; if (contact_points || penetration_depth || normal) { std::vector contacts; res = shapeIntersect(s1, tf1, s2, tf2, &contacts); if (!contacts.empty()) { // Get the deepest contact point const ContactPoint& maxDepthContact = *std::max_element(contacts.begin(), contacts.end(), comparePenDepth); if (contact_points) *contact_points = maxDepthContact.pos; if (penetration_depth) *penetration_depth = maxDepthContact.penetration_depth; if (normal) *normal = maxDepthContact.normal; } } else { res = shapeIntersect(s1, tf1, s2, tf2, NULL); } return res; } /// @brief Fast implementation for sphere-capsule collision template<> bool GJKSolver_libccd::shapeIntersect(const Sphere& s1, const Transform3f& tf1, const Capsule& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Capsule &s1, const Transform3f& tf1, const Sphere &s2, const Transform3f& tf2, std::vector* contacts) const; /// @brief Fast implementation for sphere-sphere collision template<> bool GJKSolver_libccd::shapeIntersect(const Sphere& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, std::vector* contacts) const; /// @brief Fast implementation for box-box collision template<> bool GJKSolver_libccd::shapeIntersect(const Box& s1, const Transform3f& tf1, const Box& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Sphere& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Ellipsoid& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Ellipsoid& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Box& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Box& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Capsule& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Capsule& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Cylinder& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Cylinder& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Cone& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Cone& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Sphere& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Ellipsoid& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Ellipsoid& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Box& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Box& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Capsule& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Capsule& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Cylinder& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Cylinder& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Cone& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Cone& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) const; /// @brief Fast implementation for sphere-triangle collision template<> bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; /// @brief Fast implementation for sphere-triangle collision template<> bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; template<> bool GJKSolver_libccd::shapeTriangleIntersect(const Halfspace& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; template<> bool GJKSolver_libccd::shapeTriangleIntersect(const Plane& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; /// @brief Fast implementation for sphere-capsule distance template<> bool GJKSolver_libccd::shapeDistance(const Sphere& s1, const Transform3f& tf1, const Capsule& s2, const Transform3f& tf2, FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const; template<> bool GJKSolver_libccd::shapeDistance(const Capsule& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const; /// @brief Fast implementation for sphere-sphere distance template<> bool GJKSolver_libccd::shapeDistance(const Sphere& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const; // @brief Computation of the distance result for capsule capsule. Closest points are based on two line-segments. template<> bool GJKSolver_libccd::shapeDistance(const Capsule& s1, const Transform3f& tf1, const Capsule& s2, const Transform3f& tf2, FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const; /// @brief Fast implementation for sphere-triangle distance template<> bool GJKSolver_libccd::shapeTriangleDistance(const Sphere& s, const Transform3f& tf, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const; /// @brief Fast implementation for sphere-triangle distance template<> bool GJKSolver_libccd::shapeTriangleDistance(const Sphere& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const; /// @brief collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the GJK in bullet) struct GJKSolver_indep { /// @brief intersection checking between two shapes /// @deprecated use shapeIntersect(const S1&, const Transform3f&, const S2&, const Transform3f&, std::vector*) const template FCL_DEPRECATED bool shapeIntersect(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; /// @brief intersection checking between two shapes template bool shapeIntersect(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2, std::vector* contacts = NULL) const { Vec3f guess(1, 0, 0); if(enable_cached_guess) guess = cached_guess; details::MinkowskiDiff shape; shape.shapes[0] = &s1; shape.shapes[1] = &s2; shape.toshape1 = tf2.getRotation().transposeTimes(tf1.getRotation()); shape.toshape0 = tf1.inverseTimes(tf2); details::GJK gjk(gjk_max_iterations, gjk_tolerance); details::GJK::Status gjk_status = gjk.evaluate(shape, -guess); if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex(); switch(gjk_status) { case details::GJK::Inside: { details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance); details::EPA::Status epa_status = epa.evaluate(gjk, -guess); if(epa_status != details::EPA::Failed) { Vec3f w0; for(size_t i = 0; i < epa.result.rank; ++i) { w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i]; } if(contacts) { Vec3f normal = epa.normal; Vec3f point = tf1.transform(w0 - epa.normal*(epa.depth *0.5)); FCL_REAL depth = -epa.depth; contacts->push_back(ContactPoint(normal, point, depth)); } return true; } else return false; } break; default: ; } return false; } /// @brief intersection checking between one shape and a triangle template bool shapeTriangleIntersect(const S& s, const Transform3f& tf, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points = NULL, FCL_REAL* penetration_depth = NULL, Vec3f* normal = NULL) const { TriangleP tri(P1, P2, P3); Vec3f guess(1, 0, 0); if(enable_cached_guess) guess = cached_guess; details::MinkowskiDiff shape; shape.shapes[0] = &s; shape.shapes[1] = &tri; shape.toshape1 = tf.getRotation(); shape.toshape0 = inverse(tf); details::GJK gjk(gjk_max_iterations, gjk_tolerance); details::GJK::Status gjk_status = gjk.evaluate(shape, -guess); if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex(); switch(gjk_status) { case details::GJK::Inside: { details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance); details::EPA::Status epa_status = epa.evaluate(gjk, -guess); if(epa_status != details::EPA::Failed) { Vec3f w0; for(size_t i = 0; i < epa.result.rank; ++i) { w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i]; } if(penetration_depth) *penetration_depth = -epa.depth; if(normal) *normal = -epa.normal; if(contact_points) *contact_points = tf.transform(w0 - epa.normal*(epa.depth *0.5)); return true; } else return false; } break; default: ; } return false; } //// @brief intersection checking between one shape and a triangle with transformation template bool shapeTriangleIntersect(const S& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points = NULL, FCL_REAL* penetration_depth = NULL, Vec3f* normal = NULL) const { TriangleP tri(P1, P2, P3); Vec3f guess(1, 0, 0); if(enable_cached_guess) guess = cached_guess; details::MinkowskiDiff shape; shape.shapes[0] = &s; shape.shapes[1] = &tri; shape.toshape1 = tf2.getRotation().transposeTimes(tf1.getRotation()); shape.toshape0 = tf1.inverseTimes(tf2); details::GJK gjk(gjk_max_iterations, gjk_tolerance); details::GJK::Status gjk_status = gjk.evaluate(shape, -guess); if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex(); switch(gjk_status) { case details::GJK::Inside: { details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance); details::EPA::Status epa_status = epa.evaluate(gjk, -guess); if(epa_status != details::EPA::Failed) { Vec3f w0; for(size_t i = 0; i < epa.result.rank; ++i) { w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i]; } if(penetration_depth) *penetration_depth = -epa.depth; if(normal) *normal = -epa.normal; if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5)); return true; } else return false; } break; default: ; } return false; } /// @brief distance computation between two shapes template bool shapeDistance(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2, FCL_REAL* distance = NULL, Vec3f* p1 = NULL, Vec3f* p2 = NULL) const { Vec3f guess(1, 0, 0); if(enable_cached_guess) guess = cached_guess; details::MinkowskiDiff shape; shape.shapes[0] = &s1; shape.shapes[1] = &s2; shape.toshape1 = tf2.getRotation().transposeTimes(tf1.getRotation()); shape.toshape0 = tf1.inverseTimes(tf2); details::GJK gjk(gjk_max_iterations, gjk_tolerance); details::GJK::Status gjk_status = gjk.evaluate(shape, -guess); if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex(); if(gjk_status == details::GJK::Valid) { Vec3f w0, w1; for(size_t i = 0; i < gjk.getSimplex()->rank; ++i) { FCL_REAL p = gjk.getSimplex()->p[i]; w0 += shape.support(gjk.getSimplex()->c[i]->d, 0) * p; w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p; } if(distance) *distance = (w0 - w1).length(); if(p1) *p1 = w0; if(p2) *p2 = shape.toshape0.transform(w1); return true; } else { if(distance) *distance = -1; return false; } } /// @brief distance computation between one shape and a triangle template bool shapeTriangleDistance(const S& s, const Transform3f& tf, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, FCL_REAL* distance = NULL, Vec3f* p1 = NULL, Vec3f* p2 = NULL) const { TriangleP tri(P1, P2, P3); Vec3f guess(1, 0, 0); if(enable_cached_guess) guess = cached_guess; details::MinkowskiDiff shape; shape.shapes[0] = &s; shape.shapes[1] = &tri; shape.toshape1 = tf.getRotation(); shape.toshape0 = inverse(tf); details::GJK gjk(gjk_max_iterations, gjk_tolerance); details::GJK::Status gjk_status = gjk.evaluate(shape, -guess); if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex(); if(gjk_status == details::GJK::Valid) { Vec3f w0, w1; for(size_t i = 0; i < gjk.getSimplex()->rank; ++i) { FCL_REAL p = gjk.getSimplex()->p[i]; w0 += shape.support(gjk.getSimplex()->c[i]->d, 0) * p; w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p; } if(distance) *distance = (w0 - w1).length(); if(p1) *p1 = w0; if(p2) *p2 = shape.toshape0.transform(w1); return true; } else { if(distance) *distance = -1; return false; } } /// @brief distance computation between one shape and a triangle with transformation template bool shapeTriangleDistance(const S& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, FCL_REAL* distance = NULL, Vec3f* p1 = NULL, Vec3f* p2 = NULL) const { TriangleP tri(P1, P2, P3); Vec3f guess(1, 0, 0); if(enable_cached_guess) guess = cached_guess; details::MinkowskiDiff shape; shape.shapes[0] = &s; shape.shapes[1] = &tri; shape.toshape1 = tf2.getRotation().transposeTimes(tf1.getRotation()); shape.toshape0 = tf1.inverseTimes(tf2); details::GJK gjk(gjk_max_iterations, gjk_tolerance); details::GJK::Status gjk_status = gjk.evaluate(shape, -guess); if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex(); if(gjk_status == details::GJK::Valid) { Vec3f w0, w1; for(size_t i = 0; i < gjk.getSimplex()->rank; ++i) { FCL_REAL p = gjk.getSimplex()->p[i]; w0 += shape.support(gjk.getSimplex()->c[i]->d, 0) * p; w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p; } if(distance) *distance = (w0 - w1).length(); if(p1) *p1 = w0; if(p2) *p2 = shape.toshape0.transform(w1); return true; } else { if(distance) *distance = -1; return false; } } /// @brief default setting for GJK algorithm GJKSolver_indep() { gjk_max_iterations = 128; gjk_tolerance = 1e-6; epa_max_face_num = 128; epa_max_vertex_num = 64; epa_max_iterations = 255; epa_tolerance = 1e-6; enable_cached_guess = false; cached_guess = Vec3f(1, 0, 0); } void enableCachedGuess(bool if_enable) const { enable_cached_guess = if_enable; } void setCachedGuess(const Vec3f& guess) const { cached_guess = guess; } Vec3f getCachedGuess() const { return cached_guess; } /// @brief maximum number of simplex face used in EPA algorithm unsigned int epa_max_face_num; /// @brief maximum number of simplex vertex used in EPA algorithm unsigned int epa_max_vertex_num; /// @brief maximum number of iterations used for EPA iterations unsigned int epa_max_iterations; /// @brief the threshold used in EPA to stop iteration FCL_REAL epa_tolerance; /// @brief the threshold used in GJK to stop iteration FCL_REAL gjk_tolerance; /// @brief maximum number of iterations used for GJK iterations FCL_REAL gjk_max_iterations; /// @brief Whether smart guess can be provided mutable bool enable_cached_guess; /// @brief smart guess mutable Vec3f cached_guess; }; template bool GJKSolver_indep::shapeIntersect(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const { bool res; if (contact_points || penetration_depth || normal) { std::vector contacts; res = shapeIntersect(s1, tf1, s2, tf2, &contacts); if (!contacts.empty()) { // Get the deepest contact point const ContactPoint& maxDepthContact = *std::max_element(contacts.begin(), contacts.end(), comparePenDepth); if (contact_points) *contact_points = maxDepthContact.pos; if (penetration_depth) *penetration_depth = maxDepthContact.penetration_depth; if (normal) *normal = maxDepthContact.normal; } } else { res = shapeIntersect(s1, tf1, s2, tf2, NULL); } return res; } /// @brief Fast implementation for sphere-capsule collision template<> bool GJKSolver_indep::shapeIntersect(const Sphere &s1, const Transform3f& tf1, const Capsule &s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Capsule &s1, const Transform3f& tf1, const Sphere &s2, const Transform3f& tf2, std::vector* contacts) const; /// @brief Fast implementation for sphere-sphere collision template<> bool GJKSolver_indep::shapeIntersect(const Sphere& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, std::vector* contacts) const; /// @brief Fast implementation for box-box collision template<> bool GJKSolver_indep::shapeIntersect(const Box& s1, const Transform3f& tf1, const Box& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Sphere& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Ellipsoid& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Ellipsoid& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Box& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Box& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Capsule& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Capsule& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Cylinder& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Cylinder& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Cone& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Cone& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Sphere& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Ellipsoid& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Ellipsoid& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Box& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Box& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Capsule& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Capsule& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Cylinder& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Cylinder& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Cone& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Cone& s2, const Transform3f& tf2, std::vector* contacts) const; template<> bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) const; /// @brief Fast implementation for sphere-triangle collision template<> bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; /// @brief Fast implementation for sphere-triangle collision template<> bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; template<> bool GJKSolver_indep::shapeTriangleIntersect(const Halfspace& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; template<> bool GJKSolver_indep::shapeTriangleIntersect(const Plane& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const; /// @brief Fast implementation for sphere-capsule distance template<> bool GJKSolver_indep::shapeDistance(const Sphere& s1, const Transform3f& tf1, const Capsule& s2, const Transform3f& tf2, FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const; template<> bool GJKSolver_indep::shapeDistance(const Capsule& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const; /// @brief Fast implementation for sphere-sphere distance template<> bool GJKSolver_indep::shapeDistance(const Sphere& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const; // @brief Computation of the distance result for capsule capsule. Closest points are based on two line-segments. template<> bool GJKSolver_indep::shapeDistance(const Capsule& s1, const Transform3f& tf1, const Capsule& s2, const Transform3f& tf2, FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const; /// @brief Fast implementation for sphere-triangle distance template<> bool GJKSolver_indep::shapeTriangleDistance(const Sphere& s, const Transform3f& tf, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const; /// @brief Fast implementation for sphere-triangle distance template<> bool GJKSolver_indep::shapeTriangleDistance(const Sphere& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const; } #endif fcl-0.5.0/include/fcl/octree.h000066400000000000000000000200731274356571000161150ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_OCTREE_H #define FCL_OCTREE_H #include "fcl/config.h" #if not(FCL_HAVE_OCTOMAP) #error "This header requires fcl to be compiled with octomap support" #endif #include #include #include #include "fcl/BV/AABB.h" #include "fcl/collision_object.h" namespace fcl { /// @brief Octree is one type of collision geometry which can encode uncertainty information in the sensor data. class OcTree : public CollisionGeometry { private: std::shared_ptr tree; FCL_REAL default_occupancy; FCL_REAL occupancy_threshold; FCL_REAL free_threshold; public: typedef octomap::OcTreeNode OcTreeNode; /// @brief construct octree with a given resolution OcTree(FCL_REAL resolution) : tree(std::shared_ptr(new octomap::OcTree(resolution))) { default_occupancy = tree->getOccupancyThres(); // default occupancy/free threshold is consistent with default setting from octomap occupancy_threshold = tree->getOccupancyThres(); free_threshold = 0; } /// @brief construct octree from octomap OcTree(const std::shared_ptr& tree_) : tree(tree_) { default_occupancy = tree->getOccupancyThres(); // default occupancy/free threshold is consistent with default setting from octomap occupancy_threshold = tree->getOccupancyThres(); free_threshold = 0; } /// @brief compute the AABB for the octree in its local coordinate system void computeLocalAABB() { aabb_local = getRootBV(); aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).length(); } /// @brief get the bounding volume for the root AABB getRootBV() const { FCL_REAL delta = (1 << tree->getTreeDepth()) * tree->getResolution() / 2; // std::cout << "octree size " << delta << std::endl; return AABB(Vec3f(-delta, -delta, -delta), Vec3f(delta, delta, delta)); } /// @brief get the root node of the octree OcTreeNode* getRoot() const { return tree->getRoot(); } /// @brief whether one node is completely occupied bool isNodeOccupied(const OcTreeNode* node) const { // return tree->isNodeOccupied(node); return node->getOccupancy() >= occupancy_threshold; } /// @brief whether one node is completely free bool isNodeFree(const OcTreeNode* node) const { // return false; // default no definitely free node return node->getOccupancy() <= free_threshold; } /// @brief whether one node is uncertain bool isNodeUncertain(const OcTreeNode* node) const { return (!isNodeOccupied(node)) && (!isNodeFree(node)); } /// @brief transform the octree into a bunch of boxes; uncertainty information is kept in the boxes. However, we /// only keep the occupied boxes (i.e., the boxes whose occupied probability is higher enough). std::vector > toBoxes() const { std::vector > boxes; boxes.reserve(tree->size() / 2); for(octomap::OcTree::iterator it = tree->begin(tree->getTreeDepth()), end = tree->end(); it != end; ++it) { // if(tree->isNodeOccupied(*it)) if(isNodeOccupied(&*it)) { FCL_REAL size = it.getSize(); FCL_REAL x = it.getX(); FCL_REAL y = it.getY(); FCL_REAL z = it.getZ(); FCL_REAL c = (*it).getOccupancy(); FCL_REAL t = tree->getOccupancyThres(); std::array box = {{x, y, z, size, c, t}}; boxes.push_back(box); } } return boxes; } /// @brief the threshold used to decide whether one node is occupied, this is NOT the octree occupied_thresold FCL_REAL getOccupancyThres() const { return occupancy_threshold; } /// @brief the threshold used to decide whether one node is occupied, this is NOT the octree free_threshold FCL_REAL getFreeThres() const { return free_threshold; } FCL_REAL getDefaultOccupancy() const { return default_occupancy; } void setCellDefaultOccupancy(FCL_REAL d) { default_occupancy = d; } void setOccupancyThres(FCL_REAL d) { occupancy_threshold = d; } void setFreeThres(FCL_REAL d) { free_threshold = d; } /// @return ptr to child number childIdx of node OcTreeNode* getNodeChild(OcTreeNode* node, unsigned int childIdx) { #if OCTOMAP_VERSION_AT_LEAST(1,8,0) return tree->getNodeChild(node, childIdx); #else return node->getChild(childIdx); #endif } /// @return const ptr to child number childIdx of node const OcTreeNode* getNodeChild(const OcTreeNode* node, unsigned int childIdx) const { #if OCTOMAP_VERSION_AT_LEAST(1,8,0) return tree->getNodeChild(node, childIdx); #else return node->getChild(childIdx); #endif } /// @brief return true if the child at childIdx exists bool nodeChildExists(const OcTreeNode* node, unsigned int childIdx) const { #if OCTOMAP_VERSION_AT_LEAST(1,8,0) return tree->nodeChildExists(node, childIdx); #else return node->childExists(childIdx); #endif } /// @brief return true if node has at least one child bool nodeHasChildren(const OcTreeNode* node) const { #if OCTOMAP_VERSION_AT_LEAST(1,8,0) return tree->nodeHasChildren(node); #else return node->hasChildren(); #endif } /// @brief return object type, it is an octree OBJECT_TYPE getObjectType() const { return OT_OCTREE; } /// @brief return node type, it is an octree NODE_TYPE getNodeType() const { return GEOM_OCTREE; } }; /// @brief compute the bounding volume of an octree node's i-th child static inline void computeChildBV(const AABB& root_bv, unsigned int i, AABB& child_bv) { if(i&1) { child_bv.min_[0] = (root_bv.min_[0] + root_bv.max_[0]) * 0.5; child_bv.max_[0] = root_bv.max_[0]; } else { child_bv.min_[0] = root_bv.min_[0]; child_bv.max_[0] = (root_bv.min_[0] + root_bv.max_[0]) * 0.5; } if(i&2) { child_bv.min_[1] = (root_bv.min_[1] + root_bv.max_[1]) * 0.5; child_bv.max_[1] = root_bv.max_[1]; } else { child_bv.min_[1] = root_bv.min_[1]; child_bv.max_[1] = (root_bv.min_[1] + root_bv.max_[1]) * 0.5; } if(i&4) { child_bv.min_[2] = (root_bv.min_[2] + root_bv.max_[2]) * 0.5; child_bv.max_[2] = root_bv.max_[2]; } else { child_bv.min_[2] = root_bv.min_[2]; child_bv.max_[2] = (root_bv.min_[2] + root_bv.max_[2]) * 0.5; } } } #endif fcl-0.5.0/include/fcl/profile.h000066400000000000000000000254401274356571000162770ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2008-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /* Author Ioan Sucan */ #ifndef FCL_UTIL_PROFILER #define FCL_UTIL_PROFILER #define ENABLE_PROFILING 1 #ifndef ENABLE_PROFILING /// The ENABLE_PROFILING macro can be set externally. If it is not, profiling is enabled by default, unless NDEBUG is defined. # ifdef NDEBUG # define ENABLE_PROFILING 0 # else # define ENABLE_PROFILING 1 # endif #endif #if ENABLE_PROFILING #include #include #include #include #include #include namespace fcl { /// @brief Namespace containing time datatypes and time operations namespace time { /// @brief Representation of a point in time typedef std::chrono::system_clock::time_point point; /// @brief Representation of a time duration typedef std::chrono::system_clock::duration duration; /// @brief Get the current time point inline point now(void) { return std::chrono::system_clock::now(); } /// @brief Return the time duration representing a given number of seconds inline duration seconds(double sec) { long s = (long)sec; long us = (long)((sec - s) * 1000000); return std::chrono::seconds(s) + std::chrono::microseconds(us); } /// @brief Return the number of seconds that a time duration represents inline double seconds(const duration &d) { return std::chrono::duration(d).count(); } } namespace tools { /// @brief This is a simple thread-safe tool for counting time /// spent in various chunks of code. This is different from /// external profiling tools in that it allows the user to count /// time spent in various bits of code (sub-function granularity) /// or count how many times certain pieces of code are executed. class Profiler { public: // non-copyable Profiler(const Profiler&) = delete; Profiler& operator=(const Profiler&) = delete; /// @brief This instance will call Profiler::begin() when constructed and Profiler::end() when it goes out of scope. class ScopedBlock { public: /// @brief Start counting time for the block named \e name of the profiler \e prof ScopedBlock(const std::string &name, Profiler &prof = Profiler::Instance()) : name_(name), prof_(prof) { prof_.begin(name); } ~ScopedBlock(void) { prof_.end(name_); } private: std::string name_; Profiler &prof_; }; /// @brief This instance will call Profiler::start() when constructed and Profiler::stop() when it goes out of scope. /// If the profiler was already started, this block's constructor and destructor take no action class ScopedStart { public: /// @brief Take as argument the profiler instance to operate on (\e prof) ScopedStart(Profiler &prof = Profiler::Instance()) : prof_(prof), wasRunning_(prof_.running()) { if (!wasRunning_) prof_.start(); } ~ScopedStart(void) { if (!wasRunning_) prof_.stop(); } private: Profiler &prof_; bool wasRunning_; }; /// @brief Return an instance of the class static Profiler& Instance(void); /// @brief Constructor. It is allowed to separately instantiate this /// class (not only as a singleton) Profiler(bool printOnDestroy = false, bool autoStart = false) : running_(false), printOnDestroy_(printOnDestroy) { if (autoStart) start(); } /// @brief Destructor ~Profiler(void) { if (printOnDestroy_ && !data_.empty()) status(); } /// @brief Start counting time static void Start(void) { Instance().start(); } /// @brief Stop counting time static void Stop(void) { Instance().stop(); } /// @brief Clear counted time and events static void Clear(void) { Instance().clear(); } /// @brief Start counting time void start(void); /// @brief Stop counting time void stop(void); /// @brief Clear counted time and events void clear(void); /// @brief Count a specific event for a number of times static void Event(const std::string& name, const unsigned int times = 1) { Instance().event(name, times); } /// @brief Count a specific event for a number of times void event(const std::string &name, const unsigned int times = 1); /// @brief Maintain the average of a specific value static void Average(const std::string& name, const double value) { Instance().average(name, value); } /// @brief Maintain the average of a specific value void average(const std::string &name, const double value); /// @brief Begin counting time for a specific chunk of code static void Begin(const std::string &name) { Instance().begin(name); } /// @brief Stop counting time for a specific chunk of code static void End(const std::string &name) { Instance().end(name); } /// @brief Begin counting time for a specific chunk of code void begin(const std::string &name); /// @brief Stop counting time for a specific chunk of code void end(const std::string &name); /// @brief Print the status of the profiled code chunks and /// events. Optionally, computation done by different threads /// can be printed separately. static void Status(std::ostream &out = std::cout, bool merge = true) { Instance().status(out, merge); } /// @brief Print the status of the profiled code chunks and /// events. Optionally, computation done by different threads /// can be printed separately. void status(std::ostream &out = std::cout, bool merge = true); /// @brief Check if the profiler is counting time or not bool running(void) const { return running_; } /// @brief Check if the profiler is counting time or not static bool Running(void) { return Instance().running(); } private: /// @brief Information about time spent in a section of the code struct TimeInfo { TimeInfo(void) : total(time::seconds(0.)), shortest(time::duration::max()), longest(time::duration::min()), parts(0) { } /// @brief Total time counted. time::duration total; /// @brief The shortest counted time interval time::duration shortest; /// @brief The longest counted time interval time::duration longest; /// @brief Number of times a chunk of time was added to this structure unsigned long int parts; /// @brief The point in time when counting time started time::point start; /// @brief Begin counting time void set(void) { start = time::now(); } /// @brief Add the counted time to the total time void update(void) { const time::duration &dt = time::now() - start; if (dt > longest) longest = dt; if (dt < shortest) shortest = dt; total = total + dt; ++parts; } }; /// @brief Information maintained about averaged values struct AvgInfo { /// @brief The sum of the values to average double total; /// @brief The sub of squares of the values to average double totalSqr; /// @brief Number of times a value was added to this structure unsigned long int parts; }; /// @brief Information to be maintained for each thread struct PerThread { /// @brief The stored events std::map events; /// @brief The stored averages std::map avg; /// @brief The amount of time spent in various places std::map time; }; void printThreadInfo(std::ostream &out, const PerThread &data); std::mutex lock_; std::map data_; TimeInfo tinfo_; bool running_; bool printOnDestroy_; }; } } #else #include #include /// If profiling is disabled, provide empty implementations for the public functions namespace fcl { namespace tools { class Profiler { public: class ScopedBlock { public: ScopedBlock(const std::string &, Profiler & = Profiler::Instance()) { } ~ScopedBlock(void) { } }; class ScopedStart { public: ScopedStart(Profiler & = Profiler::Instance()) { } ~ScopedStart(void) { } }; static Profiler& Instance(void); Profiler(bool = true, bool = true) { } ~Profiler(void) { } static void Start(void) { } static void Stop(void) { } static void Clear(void) { } void start(void) { } void stop(void) { } void clear(void) { } static void Event(const std::string&, const unsigned int = 1) { } void event(const std::string &, const unsigned int = 1) { } static void Average(const std::string&, const double) { } void average(const std::string &, const double) { } static void Begin(const std::string &) { } static void End(const std::string &) { } void begin(const std::string &) { } void end(const std::string &) { } static void Status(std::ostream & = std::cout, bool = true) { } void status(std::ostream & = std::cout, bool = true) { } bool running(void) const { return false; } static bool Running(void) { return false; } }; } } #endif #endif fcl-0.5.0/include/fcl/shape/000077500000000000000000000000001274356571000155615ustar00rootroot00000000000000fcl-0.5.0/include/fcl/shape/geometric_shape_to_BVH_model.h000066400000000000000000000336721274356571000234640ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef GEOMETRIC_SHAPE_TO_BVH_MODEL_H #define GEOMETRIC_SHAPE_TO_BVH_MODEL_H #include "fcl/shape/geometric_shapes.h" #include "fcl/BVH/BVH_model.h" namespace fcl { /// @brief Generate BVH model from box template void generateBVHModel(BVHModel& model, const Box& shape, const Transform3f& pose) { double a = shape.side[0]; double b = shape.side[1]; double c = shape.side[2]; std::vector points(8); std::vector tri_indices(12); points[0].setValue(0.5 * a, -0.5 * b, 0.5 * c); points[1].setValue(0.5 * a, 0.5 * b, 0.5 * c); points[2].setValue(-0.5 * a, 0.5 * b, 0.5 * c); points[3].setValue(-0.5 * a, -0.5 * b, 0.5 * c); points[4].setValue(0.5 * a, -0.5 * b, -0.5 * c); points[5].setValue(0.5 * a, 0.5 * b, -0.5 * c); points[6].setValue(-0.5 * a, 0.5 * b, -0.5 * c); points[7].setValue(-0.5 * a, -0.5 * b, -0.5 * c); tri_indices[0].set(0, 4, 1); tri_indices[1].set(1, 4, 5); tri_indices[2].set(2, 6, 3); tri_indices[3].set(3, 6, 7); tri_indices[4].set(3, 0, 2); tri_indices[5].set(2, 0, 1); tri_indices[6].set(6, 5, 7); tri_indices[7].set(7, 5, 4); tri_indices[8].set(1, 5, 2); tri_indices[9].set(2, 5, 6); tri_indices[10].set(3, 7, 0); tri_indices[11].set(0, 7, 4); for(unsigned int i = 0; i < points.size(); ++i) { points[i] = pose.transform(points[i]); } model.beginModel(); model.addSubModel(points, tri_indices); model.endModel(); model.computeLocalAABB(); } /// @brief Generate BVH model from sphere, given the number of segments along longitude and number of rings along latitude. template void generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3f& pose, unsigned int seg, unsigned int ring) { std::vector points; std::vector tri_indices; double r = shape.radius; double phi, phid; const double pi = constants::pi; phid = pi * 2 / seg; phi = 0; double theta, thetad; thetad = pi / (ring + 1); theta = 0; for(unsigned int i = 0; i < ring; ++i) { double theta_ = theta + thetad * (i + 1); for(unsigned int j = 0; j < seg; ++j) { points.push_back(Vec3f(r * sin(theta_) * cos(phi + j * phid), r * sin(theta_) * sin(phi + j * phid), r * cos(theta_))); } } points.push_back(Vec3f(0, 0, r)); points.push_back(Vec3f(0, 0, -r)); for(unsigned int i = 0; i < ring - 1; ++i) { for(unsigned int j = 0; j < seg; ++j) { unsigned int a, b, c, d; a = i * seg + j; b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1); c = (i + 1) * seg + j; d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1); tri_indices.push_back(Triangle(a, c, b)); tri_indices.push_back(Triangle(b, c, d)); } } for(unsigned int j = 0; j < seg; ++j) { unsigned int a, b; a = j; b = (j == seg - 1) ? 0 : (j + 1); tri_indices.push_back(Triangle(ring * seg, a, b)); a = (ring - 1) * seg + j; b = (j == seg - 1) ? (ring - 1) * seg : ((ring - 1) * seg + j + 1); tri_indices.push_back(Triangle(a, ring * seg + 1, b)); } for(unsigned int i = 0; i < points.size(); ++i) { points[i] = pose.transform(points[i]); } model.beginModel(); model.addSubModel(points, tri_indices); model.endModel(); model.computeLocalAABB(); } /// @brief Generate BVH model from sphere /// The difference between generateBVHModel is that it gives the number of triangles faces N for a sphere with unit radius. For sphere of radius r, /// then the number of triangles is r * r * N so that the area represented by a single triangle is approximately the same.s template void generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3f& pose, unsigned int n_faces_for_unit_sphere) { double r = shape.radius; double n_low_bound = sqrtf(n_faces_for_unit_sphere / 2.0) * r * r; unsigned int ring = ceil(n_low_bound); unsigned int seg = ceil(n_low_bound); generateBVHModel(model, shape, pose, seg, ring); } /// @brief Generate BVH model from ellipsoid, given the number of segments along longitude and number of rings along latitude. template void generateBVHModel(BVHModel& model, const Ellipsoid& shape, const Transform3f& pose, unsigned int seg, unsigned int ring) { std::vector points; std::vector tri_indices; const FCL_REAL& a = shape.radii[0]; const FCL_REAL& b = shape.radii[1]; const FCL_REAL& c = shape.radii[2]; FCL_REAL phi, phid; const FCL_REAL pi = constants::pi; phid = pi * 2 / seg; phi = 0; FCL_REAL theta, thetad; thetad = pi / (ring + 1); theta = 0; for(unsigned int i = 0; i < ring; ++i) { double theta_ = theta + thetad * (i + 1); for(unsigned int j = 0; j < seg; ++j) { points.push_back(Vec3f(a * sin(theta_) * cos(phi + j * phid), b * sin(theta_) * sin(phi + j * phid), c * cos(theta_))); } } points.push_back(Vec3f(0, 0, c)); points.push_back(Vec3f(0, 0, -c)); for(unsigned int i = 0; i < ring - 1; ++i) { for(unsigned int j = 0; j < seg; ++j) { unsigned int a, b, c, d; a = i * seg + j; b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1); c = (i + 1) * seg + j; d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1); tri_indices.push_back(Triangle(a, c, b)); tri_indices.push_back(Triangle(b, c, d)); } } for(unsigned int j = 0; j < seg; ++j) { unsigned int a, b; a = j; b = (j == seg - 1) ? 0 : (j + 1); tri_indices.push_back(Triangle(ring * seg, a, b)); a = (ring - 1) * seg + j; b = (j == seg - 1) ? (ring - 1) * seg : ((ring - 1) * seg + j + 1); tri_indices.push_back(Triangle(a, ring * seg + 1, b)); } for(unsigned int i = 0; i < points.size(); ++i) { points[i] = pose.transform(points[i]); } model.beginModel(); model.addSubModel(points, tri_indices); model.endModel(); model.computeLocalAABB(); } /// @brief Generate BVH model from ellipsoid /// The difference between generateBVHModel is that it gives the number of triangles faces N for an ellipsoid with unit radii (1, 1, 1). For ellipsoid of radii (a, b, c), /// then the number of triangles is ((a^p * b^p + b^p * c^p + c^p * a^p)/3)^(1/p) * N, where p is 1.6075, so that the area represented by a single triangle is approximately the same. /// Reference: https://en.wikipedia.org/wiki/Ellipsoid#Approximate_formula template void generateBVHModel(BVHModel& model, const Ellipsoid& shape, const Transform3f& pose, unsigned int n_faces_for_unit_ellipsoid) { const FCL_REAL p = 1.6075; const FCL_REAL& ap = std::pow(shape.radii[0], p); const FCL_REAL& bp = std::pow(shape.radii[1], p); const FCL_REAL& cp = std::pow(shape.radii[2], p); const FCL_REAL ratio = std::pow((ap * bp + bp * cp + cp * ap) / 3.0, 1.0 / p); const FCL_REAL n_low_bound = std::sqrt(n_faces_for_unit_ellipsoid / 2.0) * ratio; const unsigned int ring = std::ceil(n_low_bound); const unsigned int seg = std::ceil(n_low_bound); generateBVHModel(model, shape, pose, seg, ring); } /// @brief Generate BVH model from cylinder, given the number of segments along circle and the number of segments along axis. template void generateBVHModel(BVHModel& model, const Cylinder& shape, const Transform3f& pose, unsigned int tot, unsigned int h_num) { std::vector points; std::vector tri_indices; double r = shape.radius; double h = shape.lz; double phi, phid; const double pi = constants::pi; phid = pi * 2 / tot; phi = 0; double hd = h / h_num; for(unsigned int i = 0; i < tot; ++i) points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), h / 2)); for(unsigned int i = 0; i < h_num - 1; ++i) { for(unsigned int j = 0; j < tot; ++j) { points.push_back(Vec3f(r * cos(phi + phid * j), r * sin(phi + phid * j), h / 2 - (i + 1) * hd)); } } for(unsigned int i = 0; i < tot; ++i) points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2)); points.push_back(Vec3f(0, 0, h / 2)); points.push_back(Vec3f(0, 0, -h / 2)); for(unsigned int i = 0; i < tot; ++i) { Triangle tmp((h_num + 1) * tot, i, ((i == tot - 1) ? 0 : (i + 1))); tri_indices.push_back(tmp); } for(unsigned int i = 0; i < tot; ++i) { Triangle tmp((h_num + 1) * tot + 1, h_num * tot + ((i == tot - 1) ? 0 : (i + 1)), h_num * tot + i); tri_indices.push_back(tmp); } for(unsigned int i = 0; i < h_num; ++i) { for(unsigned int j = 0; j < tot; ++j) { int a, b, c, d; a = j; b = (j == tot - 1) ? 0 : (j + 1); c = j + tot; d = (j == tot - 1) ? tot : (j + 1 + tot); int start = i * tot; tri_indices.push_back(Triangle(start + b, start + a, start + c)); tri_indices.push_back(Triangle(start + b, start + c, start + d)); } } for(unsigned int i = 0; i < points.size(); ++i) { points[i] = pose.transform(points[i]); } model.beginModel(); model.addSubModel(points, tri_indices); model.endModel(); model.computeLocalAABB(); } /// @brief Generate BVH model from cylinder /// Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius. For cylinder with /// larger radius, the number of circle split number is r * tot. template void generateBVHModel(BVHModel& model, const Cylinder& shape, const Transform3f& pose, unsigned int tot_for_unit_cylinder) { double r = shape.radius; double h = shape.lz; const double pi = constants::pi; unsigned int tot = tot_for_unit_cylinder * r; double phid = pi * 2 / tot; double circle_edge = phid * r; unsigned int h_num = ceil(h / circle_edge); generateBVHModel(model, shape, pose, tot, h_num); } /// @brief Generate BVH model from cone, given the number of segments along circle and the number of segments along axis. template void generateBVHModel(BVHModel& model, const Cone& shape, const Transform3f& pose, unsigned int tot, unsigned int h_num) { std::vector points; std::vector tri_indices; double r = shape.radius; double h = shape.lz; double phi, phid; const double pi = constants::pi; phid = pi * 2 / tot; phi = 0; double hd = h / h_num; for(unsigned int i = 0; i < h_num - 1; ++i) { double h_i = h / 2 - (i + 1) * hd; double rh = r * (0.5 - h_i / h); for(unsigned int j = 0; j < tot; ++j) { points.push_back(Vec3f(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i)); } } for(unsigned int i = 0; i < tot; ++i) points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2)); points.push_back(Vec3f(0, 0, h / 2)); points.push_back(Vec3f(0, 0, -h / 2)); for(unsigned int i = 0; i < tot; ++i) { Triangle tmp(h_num * tot, i, (i == tot - 1) ? 0 : (i + 1)); tri_indices.push_back(tmp); } for(unsigned int i = 0; i < tot; ++i) { Triangle tmp(h_num * tot + 1, (h_num - 1) * tot + ((i == tot - 1) ? 0 : (i + 1)), (h_num - 1) * tot + i); tri_indices.push_back(tmp); } for(unsigned int i = 0; i < h_num - 1; ++i) { for(unsigned int j = 0; j < tot; ++j) { int a, b, c, d; a = j; b = (j == tot - 1) ? 0 : (j + 1); c = j + tot; d = (j == tot - 1) ? tot : (j + 1 + tot); int start = i * tot; tri_indices.push_back(Triangle(start + b, start + a, start + c)); tri_indices.push_back(Triangle(start + b, start + c, start + d)); } } for(unsigned int i = 0; i < points.size(); ++i) { points[i] = pose.transform(points[i]); } model.beginModel(); model.addSubModel(points, tri_indices); model.endModel(); model.computeLocalAABB(); } /// @brief Generate BVH model from cone /// Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius. For cone with /// larger radius, the number of circle split number is r * tot. template void generateBVHModel(BVHModel& model, const Cone& shape, const Transform3f& pose, unsigned int tot_for_unit_cone) { double r = shape.radius; double h = shape.lz; const double pi = constants::pi; unsigned int tot = tot_for_unit_cone * r; double phid = pi * 2 / tot; double circle_edge = phid * r; unsigned int h_num = ceil(h / circle_edge); generateBVHModel(model, shape, pose, tot, h_num); } } #endif fcl-0.5.0/include/fcl/shape/geometric_shapes.h000066400000000000000000000364771274356571000212740ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_GEOMETRIC_SHAPES_H #define FCL_GEOMETRIC_SHAPES_H #include "fcl/collision_object.h" #include "fcl/math/vec_3f.h" #include namespace fcl { /// @brief Base class for all basic geometric shapes class ShapeBase : public CollisionGeometry { public: ShapeBase() {} /// @brief Get object type: a geometric shape OBJECT_TYPE getObjectType() const { return OT_GEOM; } }; /// @brief Triangle stores the points instead of only indices of points class TriangleP : public ShapeBase { public: TriangleP(const Vec3f& a_, const Vec3f& b_, const Vec3f& c_) : ShapeBase(), a(a_), b(b_), c(c_) { } /// @brief virtual function of compute AABB in local coordinate void computeLocalAABB(); NODE_TYPE getNodeType() const { return GEOM_TRIANGLE; } Vec3f a, b, c; }; /// @brief Center at zero point, axis aligned box class Box : public ShapeBase { public: Box(FCL_REAL x, FCL_REAL y, FCL_REAL z) : ShapeBase(), side(x, y, z) { } Box(const Vec3f& side_) : ShapeBase(), side(side_) { } Box() {} /// @brief box side length Vec3f side; /// @brief Compute AABB void computeLocalAABB(); /// @brief Get node type: a box NODE_TYPE getNodeType() const { return GEOM_BOX; } FCL_REAL computeVolume() const { return side[0] * side[1] * side[2]; } Matrix3f computeMomentofInertia() const { FCL_REAL V = computeVolume(); FCL_REAL a2 = side[0] * side[0] * V; FCL_REAL b2 = side[1] * side[1] * V; FCL_REAL c2 = side[2] * side[2] * V; return Matrix3f((b2 + c2) / 12, 0, 0, 0, (a2 + c2) / 12, 0, 0, 0, (a2 + b2) / 12); } }; /// @brief Center at zero point sphere class Sphere : public ShapeBase { public: Sphere(FCL_REAL radius_) : ShapeBase(), radius(radius_) { } /// @brief Radius of the sphere FCL_REAL radius; /// @brief Compute AABB void computeLocalAABB(); /// @brief Get node type: a sphere NODE_TYPE getNodeType() const { return GEOM_SPHERE; } Matrix3f computeMomentofInertia() const { FCL_REAL I = 0.4 * radius * radius * computeVolume(); return Matrix3f(I, 0, 0, 0, I, 0, 0, 0, I); } FCL_REAL computeVolume() const { return 4.0 * constants::pi * radius * radius * radius / 3.0; } }; /// @brief Center at zero point ellipsoid class Ellipsoid : public ShapeBase { public: Ellipsoid(FCL_REAL a, FCL_REAL b, FCL_REAL c) : ShapeBase(), radii(a, b, c) { } Ellipsoid(const Vec3f& radii_) : ShapeBase(), radii(radii_) { } /// @brief Radii of the ellipsoid Vec3f radii; /// @brief Compute AABB void computeLocalAABB(); /// @brief Get node type: a sphere NODE_TYPE getNodeType() const { return GEOM_ELLIPSOID; } Matrix3f computeMomentofInertia() const { const FCL_REAL V = computeVolume(); const FCL_REAL a2 = radii[0] * radii[0] * V; const FCL_REAL b2 = radii[1] * radii[1] * V; const FCL_REAL c2 = radii[2] * radii[2] * V; return Matrix3f(0.2 * (b2 + c2), 0, 0, 0, 0.2 * (a2 + c2), 0, 0, 0, 0.2 * (a2 + b2)); } FCL_REAL computeVolume() const { const FCL_REAL pi = constants::pi; return 4.0 * pi * radii[0] * radii[1] * radii[2] / 3.0; } }; /// @brief Center at zero point capsule class Capsule : public ShapeBase { public: Capsule(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_) { } /// @brief Radius of capsule FCL_REAL radius; /// @brief Length along z axis FCL_REAL lz; /// @brief Compute AABB void computeLocalAABB(); /// @brief Get node type: a capsule NODE_TYPE getNodeType() const { return GEOM_CAPSULE; } FCL_REAL computeVolume() const { return constants::pi * radius * radius *(lz + radius * 4/3.0); } Matrix3f computeMomentofInertia() const { FCL_REAL v_cyl = radius * radius * lz * constants::pi; FCL_REAL v_sph = radius * radius * radius * constants::pi * 4 / 3.0; FCL_REAL ix = v_cyl * lz * lz / 12.0 + 0.25 * v_cyl * radius + 0.4 * v_sph * radius * radius + 0.25 * v_sph * lz * lz; FCL_REAL iz = (0.5 * v_cyl + 0.4 * v_sph) * radius * radius; return Matrix3f(ix, 0, 0, 0, ix, 0, 0, 0, iz); } }; /// @brief Center at zero cone class Cone : public ShapeBase { public: Cone(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_) { } /// @brief Radius of the cone FCL_REAL radius; /// @brief Length along z axis FCL_REAL lz; /// @brief Compute AABB void computeLocalAABB(); /// @brief Get node type: a cone NODE_TYPE getNodeType() const { return GEOM_CONE; } FCL_REAL computeVolume() const { return constants::pi * radius * radius * lz / 3; } Matrix3f computeMomentofInertia() const { FCL_REAL V = computeVolume(); FCL_REAL ix = V * (0.1 * lz * lz + 3 * radius * radius / 20); FCL_REAL iz = 0.3 * V * radius * radius; return Matrix3f(ix, 0, 0, 0, ix, 0, 0, 0, iz); } Vec3f computeCOM() const { return Vec3f(0, 0, -0.25 * lz); } }; /// @brief Center at zero cylinder class Cylinder : public ShapeBase { public: Cylinder(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_) { } /// @brief Radius of the cylinder FCL_REAL radius; /// @brief Length along z axis FCL_REAL lz; /// @brief Compute AABB void computeLocalAABB(); /// @brief Get node type: a cylinder NODE_TYPE getNodeType() const { return GEOM_CYLINDER; } FCL_REAL computeVolume() const { return constants::pi * radius * radius * lz; } Matrix3f computeMomentofInertia() const { FCL_REAL V = computeVolume(); FCL_REAL ix = V * (3 * radius * radius + lz * lz) / 12; FCL_REAL iz = V * radius * radius / 2; return Matrix3f(ix, 0, 0, 0, ix, 0, 0, 0, iz); } }; /// @brief Convex polytope class Convex : public ShapeBase { public: /// @brief Constructing a convex, providing normal and offset of each polytype surface, and the points and shape topology information Convex(Vec3f* plane_normals_, FCL_REAL* plane_dis_, int num_planes_, Vec3f* points_, int num_points_, int* polygons_) : ShapeBase() { plane_normals = plane_normals_; plane_dis = plane_dis_; num_planes = num_planes_; points = points_; num_points = num_points_; polygons = polygons_; edges = NULL; Vec3f sum; for(int i = 0; i < num_points; ++i) { sum += points[i]; } center = sum * (FCL_REAL)(1.0 / num_points); fillEdges(); } /// @brief Copy constructor Convex(const Convex& other) : ShapeBase(other) { plane_normals = other.plane_normals; plane_dis = other.plane_dis; num_planes = other.num_planes; points = other.points; polygons = other.polygons; edges = new Edge[other.num_edges]; memcpy(edges, other.edges, sizeof(Edge) * num_edges); } ~Convex() { delete [] edges; } /// @brief Compute AABB void computeLocalAABB(); /// @brief Get node type: a conex polytope NODE_TYPE getNodeType() const { return GEOM_CONVEX; } Vec3f* plane_normals; FCL_REAL* plane_dis; /// @brief An array of indices to the points of each polygon, it should be the number of vertices /// followed by that amount of indices to "points" in counter clockwise order int* polygons; Vec3f* points; int num_points; int num_edges; int num_planes; struct Edge { int first, second; }; Edge* edges; /// @brief center of the convex polytope, this is used for collision: center is guaranteed in the internal of the polytope (as it is convex) Vec3f center; /// based on http://number-none.com/blow/inertia/bb_inertia.doc Matrix3f computeMomentofInertia() const { Matrix3f C(0, 0, 0, 0, 0, 0, 0, 0, 0); Matrix3f C_canonical(1/60.0, 1/120.0, 1/120.0, 1/120.0, 1/60.0, 1/120.0, 1/120.0, 1/120.0, 1/60.0); int* points_in_poly = polygons; int* index = polygons + 1; for(int i = 0; i < num_planes; ++i) { Vec3f plane_center; // compute the center of the polygon for(int j = 0; j < *points_in_poly; ++j) plane_center += points[index[j]]; plane_center = plane_center * (1.0 / *points_in_poly); // compute the volume of tetrahedron making by neighboring two points, the plane center and the reference point (zero) of the convex shape const Vec3f& v3 = plane_center; for(int j = 0; j < *points_in_poly; ++j) { int e_first = index[j]; int e_second = index[(j+1)%*points_in_poly]; const Vec3f& v1 = points[e_first]; const Vec3f& v2 = points[e_second]; FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3); Matrix3f A(v1, v2, v3); // this is A' in the original document C += transpose(A) * C_canonical * A * d_six_vol; // change accordingly } points_in_poly += (*points_in_poly + 1); index = points_in_poly + 1; } FCL_REAL trace_C = C(0, 0) + C(1, 1) + C(2, 2); return Matrix3f(trace_C - C(0, 0), -C(0, 1), -C(0, 2), -C(1, 0), trace_C - C(1, 1), -C(1, 2), -C(2, 0), -C(2, 1), trace_C - C(2, 2)); } Vec3f computeCOM() const { Vec3f com; FCL_REAL vol = 0; int* points_in_poly = polygons; int* index = polygons + 1; for(int i = 0; i < num_planes; ++i) { Vec3f plane_center; // compute the center of the polygon for(int j = 0; j < *points_in_poly; ++j) plane_center += points[index[j]]; plane_center = plane_center * (1.0 / *points_in_poly); // compute the volume of tetrahedron making by neighboring two points, the plane center and the reference point (zero) of the convex shape const Vec3f& v3 = plane_center; for(int j = 0; j < *points_in_poly; ++j) { int e_first = index[j]; int e_second = index[(j+1)%*points_in_poly]; const Vec3f& v1 = points[e_first]; const Vec3f& v2 = points[e_second]; FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3); vol += d_six_vol; com += (points[e_first] + points[e_second] + plane_center) * d_six_vol; } points_in_poly += (*points_in_poly + 1); index = points_in_poly + 1; } return com / (vol * 4); // here we choose zero as the reference } FCL_REAL computeVolume() const { FCL_REAL vol = 0; int* points_in_poly = polygons; int* index = polygons + 1; for(int i = 0; i < num_planes; ++i) { Vec3f plane_center; // compute the center of the polygon for(int j = 0; j < *points_in_poly; ++j) plane_center += points[index[j]]; plane_center = plane_center * (1.0 / *points_in_poly); // compute the volume of tetrahedron making by neighboring two points, the plane center and the reference point (zero point) of the convex shape const Vec3f& v3 = plane_center; for(int j = 0; j < *points_in_poly; ++j) { int e_first = index[j]; int e_second = index[(j+1)%*points_in_poly]; const Vec3f& v1 = points[e_first]; const Vec3f& v2 = points[e_second]; FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3); vol += d_six_vol; } points_in_poly += (*points_in_poly + 1); index = points_in_poly + 1; } return vol / 6; } protected: /// @brief Get edge information void fillEdges(); }; /// @brief Half Space: this is equivalent to the Plane in ODE. The separation plane is defined as n * x = d; /// Points in the negative side of the separation plane (i.e. {x | n * x < d}) are inside the half space and points /// in the positive side of the separation plane (i.e. {x | n * x > d}) are outside the half space class Halfspace : public ShapeBase { public: /// @brief Construct a half space with normal direction and offset Halfspace(const Vec3f& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_) { unitNormalTest(); } /// @brief Construct a plane with normal direction and offset Halfspace(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d_) : ShapeBase(), n(a, b, c), d(d_) { unitNormalTest(); } Halfspace() : ShapeBase(), n(1, 0, 0), d(0) { } FCL_REAL signedDistance(const Vec3f& p) const { return n.dot(p) - d; } FCL_REAL distance(const Vec3f& p) const { return std::abs(n.dot(p) - d); } /// @brief Compute AABB void computeLocalAABB(); /// @brief Get node type: a half space NODE_TYPE getNodeType() const { return GEOM_HALFSPACE; } /// @brief Plane normal Vec3f n; /// @brief Plane offset FCL_REAL d; protected: /// @brief Turn non-unit normal into unit void unitNormalTest(); }; /// @brief Infinite plane class Plane : public ShapeBase { public: /// @brief Construct a plane with normal direction and offset Plane(const Vec3f& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_) { unitNormalTest(); } /// @brief Construct a plane with normal direction and offset Plane(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d_) : ShapeBase(), n(a, b, c), d(d_) { unitNormalTest(); } Plane() : ShapeBase(), n(1, 0, 0), d(0) {} FCL_REAL signedDistance(const Vec3f& p) const { return n.dot(p) - d; } FCL_REAL distance(const Vec3f& p) const { return std::abs(n.dot(p) - d); } /// @brief Compute AABB void computeLocalAABB(); /// @brief Get node type: a plane NODE_TYPE getNodeType() const { return GEOM_PLANE; } /// @brief Plane normal Vec3f n; /// @brief Plane offset FCL_REAL d; protected: /// @brief Turn non-unit normal into unit void unitNormalTest(); }; } #endif fcl-0.5.0/include/fcl/shape/geometric_shapes_utility.h000066400000000000000000000165051274356571000230450ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_GEOMETRIC_SHAPES_UTILITY_H #define FCL_GEOMETRIC_SHAPES_UTILITY_H #include #include "fcl/shape/geometric_shapes.h" #include "fcl/BV/BV.h" namespace fcl { /// @cond IGNORE namespace details { /// @brief get the vertices of some convex shape which can bound the given shape in a specific configuration std::vector getBoundVertices(const Box& box, const Transform3f& tf); std::vector getBoundVertices(const Sphere& sphere, const Transform3f& tf); std::vector getBoundVertices(const Ellipsoid& ellipsoid, const Transform3f& tf); std::vector getBoundVertices(const Capsule& capsule, const Transform3f& tf); std::vector getBoundVertices(const Cone& cone, const Transform3f& tf); std::vector getBoundVertices(const Cylinder& cylinder, const Transform3f& tf); std::vector getBoundVertices(const Convex& convex, const Transform3f& tf); std::vector getBoundVertices(const TriangleP& triangle, const Transform3f& tf); } /// @endcond /// @brief calculate a bounding volume for a shape in a specific configuration template void computeBV(const S& s, const Transform3f& tf, BV& bv) { std::vector convex_bound_vertices = details::getBoundVertices(s, tf); fit(&convex_bound_vertices[0], (int)convex_bound_vertices.size(), bv); } template<> void computeBV(const Box& s, const Transform3f& tf, AABB& bv); template<> void computeBV(const Sphere& s, const Transform3f& tf, AABB& bv); template<> void computeBV(const Ellipsoid& s, const Transform3f& tf, AABB& bv); template<> void computeBV(const Capsule& s, const Transform3f& tf, AABB& bv); template<> void computeBV(const Cone& s, const Transform3f& tf, AABB& bv); template<> void computeBV(const Cylinder& s, const Transform3f& tf, AABB& bv); template<> void computeBV(const Convex& s, const Transform3f& tf, AABB& bv); template<> void computeBV(const TriangleP& s, const Transform3f& tf, AABB& bv); template<> void computeBV(const Halfspace& s, const Transform3f& tf, AABB& bv); template<> void computeBV(const Plane& s, const Transform3f& tf, AABB& bv); template<> void computeBV(const Box& s, const Transform3f& tf, OBB& bv); template<> void computeBV(const Sphere& s, const Transform3f& tf, OBB& bv); template<> void computeBV(const Ellipsoid& s, const Transform3f& tf, OBB& bv); template<> void computeBV(const Capsule& s, const Transform3f& tf, OBB& bv); template<> void computeBV(const Cone& s, const Transform3f& tf, OBB& bv); template<> void computeBV(const Cylinder& s, const Transform3f& tf, OBB& bv); template<> void computeBV(const Convex& s, const Transform3f& tf, OBB& bv); template<> void computeBV(const Halfspace& s, const Transform3f& tf, OBB& bv); template<> void computeBV(const Halfspace& s, const Transform3f& tf, RSS& bv); template<> void computeBV(const Halfspace& s, const Transform3f& tf, OBBRSS& bv); template<> void computeBV(const Halfspace& s, const Transform3f& tf, kIOS& bv); template<> void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<16>& bv); template<> void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<18>& bv); template<> void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<24>& bv); template<> void computeBV(const Plane& s, const Transform3f& tf, OBB& bv); template<> void computeBV(const Plane& s, const Transform3f& tf, RSS& bv); template<> void computeBV(const Plane& s, const Transform3f& tf, OBBRSS& bv); template<> void computeBV(const Plane& s, const Transform3f& tf, kIOS& bv); template<> void computeBV, Plane>(const Plane& s, const Transform3f& tf, KDOP<16>& bv); template<> void computeBV, Plane>(const Plane& s, const Transform3f& tf, KDOP<18>& bv); template<> void computeBV, Plane>(const Plane& s, const Transform3f& tf, KDOP<24>& bv); /// @brief construct a box shape (with a configuration) from a given bounding volume void constructBox(const AABB& bv, Box& box, Transform3f& tf); void constructBox(const OBB& bv, Box& box, Transform3f& tf); void constructBox(const OBBRSS& bv, Box& box, Transform3f& tf); void constructBox(const kIOS& bv, Box& box, Transform3f& tf); void constructBox(const RSS& bv, Box& box, Transform3f& tf); void constructBox(const KDOP<16>& bv, Box& box, Transform3f& tf); void constructBox(const KDOP<18>& bv, Box& box, Transform3f& tf); void constructBox(const KDOP<24>& bv, Box& box, Transform3f& tf); void constructBox(const AABB& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf); void constructBox(const OBB& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf); void constructBox(const OBBRSS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf); void constructBox(const kIOS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf); void constructBox(const RSS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf); void constructBox(const KDOP<16>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf); void constructBox(const KDOP<18>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf); void constructBox(const KDOP<24>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf); Halfspace transform(const Halfspace& a, const Transform3f& tf); Plane transform(const Plane& a, const Transform3f& tf); } #endif fcl-0.5.0/include/fcl/simd/000077500000000000000000000000001274356571000154155ustar00rootroot00000000000000fcl-0.5.0/include/fcl/simd/math_simd_details.h000066400000000000000000001074631274356571000212530ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_MATH_SIMD_DETAILS_H #define FCL_MATH_SIMD_DETAILS_H #include "fcl/data_types.h" #include #if defined (__SSE3__) #include #endif #if defined (__SSE4__) #include #endif namespace fcl { /** \brief FCL internals. Ignore this :) unless you are God */ namespace details { const __m128 xmms_0 = {0.f, 0.f, 0.f, 0.f}; const __m128d xmmd_0 = {0, 0}; static inline __m128 vec_sel(__m128 a, __m128 b, __m128 mask) { return _mm_or_ps(_mm_and_ps(mask, b), _mm_andnot_ps(mask, a)); } static inline __m128 vec_sel(__m128 a, __m128 b, const unsigned int* mask) { return vec_sel(a, b, _mm_load_ps((float*)mask)); } static inline __m128 vec_sel(__m128 a, __m128 b, unsigned int mask) { return vec_sel(a, b, _mm_set1_ps(*(float*)&mask)); } #define vec_splat(a, e) _mm_shuffle_ps((a), (a), _MM_SHUFFLE((e), (e), (e), (e))) #define vec_splatd(a, e) _mm_shuffle_pd((a), (a), _MM_SHUFFLE2((e), (e))) #define _mm_ror_ps(x, e) (((e) % 4) ? _mm_shuffle_ps((x), (x), _MM_SHUFFLE(((e)+3)%4, ((e)+2)%4, ((e)+1)%4, (e)%4)) : (x)) #define _mm_rol_ps(x, e) (((e) % 4) ? _mm_shuffle_ps((x), (x), _MM_SHUFFLE((7-(e))%4, (6-(e))%4, (5-(e))%4, (4-(e))%4)) : (x)) static inline __m128 newtonraphson_rsqrt4(const __m128 v) { static const union { float i[4]; __m128 m; } _half4 __attribute__ ((aligned(16))) = {{.5f, .5f, .5f, .5f}}; static const union { float i[4]; __m128 m; } _three __attribute__ ((aligned(16))) = {{3.f, 3.f, 3.f, 3.f}}; __m128 approx = _mm_rsqrt_ps(v); __m128 muls = _mm_mul_ps(_mm_mul_ps(v, approx), approx); return _mm_mul_ps(_mm_mul_ps(_half4.m, approx), _mm_sub_ps(_three.m, muls)); } struct sse_meta_f4 { typedef float meta_type; union {float vs[4]; __m128 v; }; sse_meta_f4() : v(_mm_set1_ps(0)) {} sse_meta_f4(float x) : v(_mm_set1_ps(x)) {} sse_meta_f4(float* px) : v(_mm_load_ps(px)) {} sse_meta_f4(__m128 x) : v(x) {} sse_meta_f4(float x, float y, float z, float w = 1) : v(_mm_setr_ps(x, y, z, w)) {} inline void setValue(float x, float y, float z, float w = 1) { v = _mm_setr_ps(x, y, z, w); } inline void setValue(float x) { v = _mm_set1_ps(x); } inline void setValue(__m128 x) { v = x; } inline void negate() { v = _mm_sub_ps(xmms_0, v); } inline sse_meta_f4& ubound(const sse_meta_f4& u) { v = _mm_min_ps(v, u.v); return *this; } inline sse_meta_f4& lbound(const sse_meta_f4& l) { v = _mm_max_ps(v, l.v); return *this; } inline void* operator new [] (size_t n) { return _mm_malloc(n, 16); } inline void operator delete [] (void* x) { if(x) _mm_free(x); } inline float operator [] (size_t i) const { return vs[i]; } inline float& operator [] (size_t i) { return vs[i]; } inline sse_meta_f4 operator + (const sse_meta_f4& other) const { return sse_meta_f4(_mm_add_ps(v, other.v)); } inline sse_meta_f4 operator - (const sse_meta_f4& other) const { return sse_meta_f4(_mm_sub_ps(v, other.v)); } inline sse_meta_f4 operator * (const sse_meta_f4& other) const { return sse_meta_f4(_mm_mul_ps(v, other.v)); } inline sse_meta_f4 operator / (const sse_meta_f4& other) const { return sse_meta_f4(_mm_div_ps(v, other.v)); } inline sse_meta_f4& operator += (const sse_meta_f4& other) { v = _mm_add_ps(v, other.v); return *this; } inline sse_meta_f4& operator -= (const sse_meta_f4& other) { v = _mm_sub_ps(v, other.v); return *this; } inline sse_meta_f4& operator *= (const sse_meta_f4& other) { v = _mm_mul_ps(v, other.v); return *this; } inline sse_meta_f4& operator /= (const sse_meta_f4& other) { v = _mm_div_ps(v, other.v); return *this; } inline sse_meta_f4 operator + (float t) const { return sse_meta_f4(_mm_add_ps(v, _mm_set1_ps(t))); } inline sse_meta_f4 operator - (float t) const { return sse_meta_f4(_mm_sub_ps(v, _mm_set1_ps(t))); } inline sse_meta_f4 operator * (float t) const { return sse_meta_f4(_mm_mul_ps(v, _mm_set1_ps(t))); } inline sse_meta_f4 operator / (float t) const { return sse_meta_f4(_mm_div_ps(v, _mm_set1_ps(t))); } inline sse_meta_f4& operator += (float t) { v = _mm_add_ps(v, _mm_set1_ps(t)); return *this; } inline sse_meta_f4& operator -= (float t) { v = _mm_sub_ps(v, _mm_set1_ps(t)); return *this; } inline sse_meta_f4& operator *= (float t) { v = _mm_mul_ps(v, _mm_set1_ps(t)); return *this; } inline sse_meta_f4& operator /= (float t) { v = _mm_div_ps(v, _mm_set1_ps(t)); return *this; } inline sse_meta_f4 operator - () const { static const union { int i[4]; __m128 m; } negativemask __attribute__ ((aligned(16))) = {{0x80000000, 0x80000000, 0x80000000, 0x80000000}}; return sse_meta_f4(_mm_xor_ps(negativemask.m, v)); } } __attribute__ ((aligned (16))); struct sse_meta_d4 { typedef double meta_type; union {double vs[4]; __m128d v[2]; }; sse_meta_d4() { setValue(0.0); } sse_meta_d4(double x) { setValue(x); } sse_meta_d4(double* px) { v[0] = _mm_load_pd(px); v[1] = _mm_set_pd(0, *(px + 2)); } sse_meta_d4(__m128d x, __m128d y) { v[0] = x; v[1] = y; } sse_meta_d4(double x, double y, double z, double w = 0) { setValue(x, y, z, w); } inline void setValue(double x, double y, double z, double w = 0) { v[0] = _mm_setr_pd(x, y); v[1] = _mm_setr_pd(z, w); } inline void setValue(double x) { v[0] = _mm_set1_pd(x); v[1] = v[0]; } inline void setValue(__m128d x, __m128d y) { v[0] = x; v[1] = y; } inline void negate() { v[0] = _mm_sub_pd(xmmd_0, v[0]); v[1] = _mm_sub_pd(xmmd_0, v[1]); } inline sse_meta_d4& ubound(const sse_meta_d4& u) { v[0] = _mm_min_pd(v[0], u.v[0]); v[1] = _mm_min_pd(v[1], u.v[1]); return *this; } inline sse_meta_d4& lbound(const sse_meta_d4& l) { v[0] = _mm_max_pd(v[0], l.v[0]); v[1] = _mm_max_pd(v[1], l.v[1]); return *this; } inline void* operator new [] (size_t n) { return _mm_malloc(n, 16); } inline void operator delete [] (void* x) { if(x) _mm_free(x); } inline double operator [] (size_t i) const { return vs[i]; } inline double& operator [] (size_t i) { return vs[i]; } inline sse_meta_d4 operator + (const sse_meta_d4& other) const { return sse_meta_d4(_mm_add_pd(v[0], other.v[0]), _mm_add_pd(v[1], other.v[1])); } inline sse_meta_d4 operator - (const sse_meta_d4& other) const { return sse_meta_d4(_mm_sub_pd(v[0], other.v[0]), _mm_sub_pd(v[1], other.v[1])); } inline sse_meta_d4 operator * (const sse_meta_d4& other) const { return sse_meta_d4(_mm_mul_pd(v[0], other.v[0]), _mm_mul_pd(v[1], other.v[1])); } inline sse_meta_d4 operator / (const sse_meta_d4& other) const { return sse_meta_d4(_mm_div_pd(v[0], other.v[0]), _mm_div_pd(v[1], other.v[1])); } inline sse_meta_d4& operator += (const sse_meta_d4& other) { v[0] = _mm_add_pd(v[0], other.v[0]); v[1] = _mm_add_pd(v[1], other.v[1]); return *this; } inline sse_meta_d4& operator -= (const sse_meta_d4& other) { v[0] = _mm_sub_pd(v[0], other.v[0]); v[1] = _mm_sub_pd(v[1], other.v[1]); return *this; } inline sse_meta_d4& operator *= (const sse_meta_d4& other) { v[0] = _mm_mul_pd(v[0], other.v[0]); v[1] = _mm_mul_pd(v[1], other.v[1]); return *this; } inline sse_meta_d4& operator /= (const sse_meta_d4& other) { v[0] = _mm_div_pd(v[0], other.v[0]); v[1] = _mm_div_pd(v[1], other.v[1]); return *this; } inline sse_meta_d4 operator + (double t) const { register __m128d d = _mm_set1_pd(t); return sse_meta_d4(_mm_add_pd(v[0], d), _mm_add_pd(v[1], d)); } inline sse_meta_d4 operator - (double t) const { register __m128d d = _mm_set1_pd(t); return sse_meta_d4(_mm_sub_pd(v[0], d), _mm_sub_pd(v[1], d)); } inline sse_meta_d4 operator * (double t) const { register __m128d d = _mm_set1_pd(t); return sse_meta_d4(_mm_mul_pd(v[0], d), _mm_mul_pd(v[1], d)); } inline sse_meta_d4 operator / (double t) const { register __m128d d = _mm_set1_pd(t); return sse_meta_d4(_mm_div_pd(v[0], d), _mm_div_pd(v[1], d)); } inline sse_meta_d4& operator += (double t) { register __m128d d = _mm_set1_pd(t); v[0] = _mm_add_pd(v[0], d); v[1] = _mm_add_pd(v[1], d); return *this; } inline sse_meta_d4& operator -= (double t) { register __m128d d = _mm_set1_pd(t); v[0] = _mm_sub_pd(v[0], d); v[1] = _mm_sub_pd(v[1], d); return *this; } inline sse_meta_d4& operator *= (double t) { register __m128d d = _mm_set1_pd(t); v[0] = _mm_mul_pd(v[0], d); v[1] = _mm_mul_pd(v[1], d); return *this; } inline sse_meta_d4& operator /= (double t) { register __m128d d = _mm_set1_pd(t); v[0] = _mm_div_pd(v[0], d); v[1] = _mm_div_pd(v[1], d); return *this; } inline sse_meta_d4 operator - () const { static const union { FCL_INT64 i[2]; __m128d m; } negativemask __attribute__ ((aligned(16))) = {{0x8000000000000000, 0x8000000000000000}}; return sse_meta_d4(_mm_xor_pd(v[0], negativemask.m), _mm_xor_pd(v[1], negativemask.m)); } } __attribute__ ((aligned (16))); static inline __m128 cross_prod(__m128 x, __m128 y) { // set to a[1][2][0][3] , b[2][0][1][3] // multiply static const int s1 = _MM_SHUFFLE(3, 0, 2, 1); static const int s2 = _MM_SHUFFLE(3, 1, 0, 2); __m128 xa = _mm_mul_ps(_mm_shuffle_ps(x, x, s1), _mm_shuffle_ps(y, y, s2)); // set to a[2][0][1][3] , b[1][2][0][3] // multiply __m128 xb = _mm_mul_ps(_mm_shuffle_ps(x, x, s2), _mm_shuffle_ps(y, y, s1)); // subtract return _mm_sub_ps(xa, xb); } static inline sse_meta_f4 cross_prod(const sse_meta_f4& x, const sse_meta_f4& y) { return sse_meta_f4(cross_prod(x.v, y.v)); } static inline void cross_prod(__m128d x0, __m128d x1, __m128d y0, __m128d y1, __m128d* z0, __m128d* z1) { static const int s0 = _MM_SHUFFLE2(0, 0); static const int s1 = _MM_SHUFFLE2(0, 1); static const int s2 = _MM_SHUFFLE2(1, 0); static const int s3 = _MM_SHUFFLE2(1, 1); __m128d xa1 = _mm_mul_pd(_mm_shuffle_pd(x0, x1, s1), _mm_shuffle_pd(y1, y0, s0)); __m128d ya1 = _mm_mul_pd(_mm_shuffle_pd(x0, x1, s2), _mm_shuffle_pd(y0, y1, s3)); __m128d xa2 = _mm_mul_pd(_mm_shuffle_pd(x1, x0, s0), _mm_shuffle_pd(y0, y1, s1)); __m128d ya2 = _mm_mul_pd(_mm_shuffle_pd(x0, x1, s3), _mm_shuffle_pd(y0, y1, s2)); *z0 = _mm_sub_pd(xa1, xa2); *z1 = _mm_sub_pd(ya1, ya2); } static inline sse_meta_d4 cross_prod(const sse_meta_d4& x, const sse_meta_d4& y) { __m128d z0, z1; cross_prod(x.v[0], x.v[1], y.v[0], y.v[1], &z0, &z1); return sse_meta_d4(z0, z1); } static inline __m128 dot_prod3(__m128 x, __m128 y) { register __m128 m = _mm_mul_ps(x, y); return _mm_add_ps(_mm_shuffle_ps(m, m, _MM_SHUFFLE(0, 0, 0, 0)), _mm_add_ps(vec_splat(m, 1), vec_splat(m, 2))); } static inline float dot_prod3(const sse_meta_f4& x, const sse_meta_f4& y) { return _mm_cvtss_f32(dot_prod3(x.v, y.v)); } static inline __m128d dot_prod3(__m128d x0, __m128d x1, __m128d y0, __m128d y1) { register __m128d m1 = _mm_mul_pd(x0, y0); register __m128d m2 = _mm_mul_pd(x1, y1); return _mm_add_pd(_mm_add_pd(vec_splatd(m1, 0), vec_splatd(m1, 1)), vec_splatd(m2, 0)); } static inline double dot_prod3(const sse_meta_d4& x, const sse_meta_d4& y) { double d; _mm_storel_pd(&d, dot_prod3(x.v[0], x.v[1], y.v[0], y.v[1])); return d; } static inline __m128 dot_prod4(__m128 x, __m128 y) { #if defined (__SSE4__) return _mm_dp_ps(x, y, 0x71); #elif defined (__SSE3__) register __m128 t = _mm_mul_ps(x, y); t = _mm_hadd_ps(t, t); return _mm_hadd_ps(t, t); #else register __m128 s = _mm_mul_ps(x, y); register __m128 r = _mm_add_ss(s, _mm_movehl_ps(s, s)); return _mm_add_ss(r, _mm_shuffle_ps(r, r, 1)); #endif } static inline float dot_prod4(const sse_meta_f4& x, const sse_meta_f4& y) { return _mm_cvtss_f32(dot_prod4(x.v, y.v)); } static inline __m128d dot_prod4(__m128d x0, __m128d x1, __m128d y0, __m128d y1) { #if defined (__SSE4__) register __m128d t1 = _mm_dp_pd(x0, y0, 0x31); register __m128d t2 = _mm_dp_pd(x1, y1, 0x11); return _mm_add_pd(t1, t2); #elif defined (__SSE3__) register __m128d t1 = _mm_mul_pd(x0, y0); register __m128d t2 = _mm_mul_pd(x1, y1); t1 = _mm_hadd_pd(t1, t1); t2 = _mm_hadd_pd(t2, t2); return _mm_add_pd(t1, t2); #else register __m128d t1 = _mm_mul_pd(x0, y0); register __m128d t2 = _mm_mul_pd(x1, y1); t1 = _mm_add_pd(t1, t2); return _mm_add_pd(t1, _mm_shuffle_pd(t1, t1, 1)); #endif } static inline double dot_prod4(const sse_meta_d4& x, const sse_meta_d4& y) { double d; _mm_storel_pd(&d, dot_prod4(x.v[0], x.v[1], y.v[0], y.v[1])); return d; } static inline sse_meta_f4 min(const sse_meta_f4& x, const sse_meta_f4& y) { return sse_meta_f4(_mm_min_ps(x.v, y.v)); } static inline sse_meta_d4 min(const sse_meta_d4& x, const sse_meta_d4& y) { return sse_meta_d4(_mm_min_pd(x.v[0], y.v[0]), _mm_min_pd(x.v[1], y.v[1])); } static inline sse_meta_f4 max(const sse_meta_f4& x, const sse_meta_f4& y) { return sse_meta_f4(_mm_max_ps(x.v, y.v)); } static inline sse_meta_d4 max(const sse_meta_d4& x, const sse_meta_d4& y) { return sse_meta_d4(_mm_max_pd(x.v[0], y.v[0]), _mm_max_pd(x.v[1], y.v[1])); } static inline sse_meta_f4 abs(const sse_meta_f4& x) { static const union { int i[4]; __m128 m; } abs4mask __attribute__ ((aligned (16))) = {{0x7fffffff, 0x7fffffff, 0x7fffffff, 0x7fffffff}}; return sse_meta_f4(_mm_and_ps(x.v, abs4mask.m)); } static inline sse_meta_d4 abs(const sse_meta_d4& x) { static const union { FCL_INT64 i[2]; __m128d m; } abs2mask __attribute__ ((aligned (16))) = {{0x7fffffffffffffff, 0x7fffffffffffffff}}; return sse_meta_d4(_mm_and_pd(x.v[0], abs2mask.m), _mm_and_pd(x.v[1], abs2mask.m)); } static inline bool equal(const sse_meta_f4& x, const sse_meta_f4& y, float epsilon) { register __m128 d = _mm_sub_ps(x.v, y.v); register __m128 e = _mm_set1_ps(epsilon); return ((_mm_movemask_ps(_mm_cmplt_ps(d, e)) & 0x7) == 0x7) && ((_mm_movemask_ps(_mm_cmpgt_ps(d, _mm_sub_ps(xmms_0, e))) & 0x7) == 0x7); } static inline bool equal(const sse_meta_d4& x, const sse_meta_d4& y, double epsilon) { register __m128d d = _mm_sub_pd(x.v[0], y.v[0]); register __m128d e = _mm_set1_pd(epsilon); if(_mm_movemask_pd(_mm_cmplt_pd(d, e)) != 0x3) return false; if(_mm_movemask_pd(_mm_cmpgt_pd(d, _mm_sub_pd(xmmd_0, e))) != 0x3) return false; d = _mm_sub_pd(x.v[1], y.v[1]); if((_mm_movemask_pd(_mm_cmplt_pd(d, e)) & 0x1) != 0x1) return false; if((_mm_movemask_pd(_mm_cmpgt_pd(d, _mm_sub_pd(xmmd_0, e))) & 0x1) != 0x1) return false; return true; } static inline sse_meta_f4 normalize3(const sse_meta_f4& x) { register __m128 m = _mm_mul_ps(x.v, x.v); __m128 r = _mm_add_ps(vec_splat(m, 0), _mm_add_ps(vec_splat(m, 1), vec_splat(m, 2))); return sse_meta_f4(_mm_mul_ps(x.v, newtonraphson_rsqrt4(r))); } static inline sse_meta_f4 normalize3_approx(const sse_meta_f4& x) { register __m128 m = _mm_mul_ps(x.v, x.v); __m128 r = _mm_add_ps(vec_splat(m, 0), _mm_add_ps(vec_splat(m, 1), vec_splat(m, 2))); return sse_meta_f4(_mm_mul_ps(x.v, _mm_rsqrt_ps(r))); } static inline void transpose(__m128 c0, __m128 c1, __m128 c2, __m128* r0, __m128* r1, __m128* r2) { static const union { unsigned int i[4]; __m128 m; } selectmask __attribute__ ((aligned(16))) = {{0, 0xffffffff, 0, 0}}; register __m128 t0, t1; t0 = _mm_unpacklo_ps(c0, c2); t1 = _mm_unpackhi_ps(c0, c2); *r0 = _mm_unpacklo_ps(t0, c1); *r1 = _mm_shuffle_ps(t0, t0, _MM_SHUFFLE(0, 3, 2, 2)); *r1 = vec_sel(*r1, c1, selectmask.i); *r2 = _mm_shuffle_ps(t1, t1, _MM_SHUFFLE(0, 1, 1, 0)); *r2 = vec_sel(*r2, vec_splat(c1, 2), selectmask.i); } static inline void inverse(__m128 c0, __m128 c1, __m128 c2, __m128* i0, __m128* i1, __m128* i2) { __m128 t0, t1, t2, d, invd; t2 = cross_prod(c0, c1); t0 = cross_prod(c1, c2); t1 = cross_prod(c2, c0); d = dot_prod3(t2, c2); d = vec_splat(d, 0); invd = _mm_rcp_ps(d); // approximate inverse transpose(t0, t1, t2, i0, i1, i2); *i0 = _mm_mul_ps(*i0, invd); *i1 = _mm_mul_ps(*i1, invd); *i2 = _mm_mul_ps(*i2, invd); } struct sse_meta_f12 { typedef float meta_type; typedef sse_meta_f4 vector_type; sse_meta_f4 c[3]; sse_meta_f12() { setZero(); } sse_meta_f12(float xx, float xy, float xz, float yx, float yy, float yz, float zx, float zy, float zz) { setValue(xx, xy, xz, yx, yy, yz, zx, zy, zz); } sse_meta_f12(const sse_meta_f4& x, const sse_meta_f4& y, const sse_meta_f4& z) { setColumn(x, y, z); } sse_meta_f12(__m128 x, __m128 y, __m128 z) { setColumn(x, y, z); } inline void setValue(float xx, float xy, float xz, float yx, float yy, float yz, float zx, float zy, float zz) { c[0].setValue(xx, yx, zx, 0); c[1].setValue(xy, yy, zy, 0); c[2].setValue(xz, yz, zz, 0); } inline void setIdentity() { c[0].setValue(1, 0, 0, 0); c[1].setValue(0, 1, 0, 0); c[2].setValue(0, 0, 1, 0); } inline void setZero() { c[0].setValue(0); c[1].setValue(0); c[2].setValue(0); } inline void setColumn(const sse_meta_f4& x, const sse_meta_f4& y, const sse_meta_f4& z) { c[0] = x; c[1] = y; c[2] = z; } inline void setColumn(__m128 x, __m128 y, __m128 z) { c[0].setValue(x); c[1].setValue(y); c[2].setValue(z); } inline const sse_meta_f4& getColumn(size_t i) const { return c[i]; } inline sse_meta_f4& getColumn(size_t i) { return c[i]; } inline sse_meta_f4 getRow(size_t i) const { return sse_meta_f4(c[0][i], c[1][i], c[2][i], 0); } inline float operator () (size_t i, size_t j) const { return c[j][i]; } inline float& operator () (size_t i, size_t j) { return c[j][i]; } inline sse_meta_f4 operator * (const sse_meta_f4& v) const { return sse_meta_f4(_mm_add_ps(_mm_add_ps(_mm_mul_ps(c[0].v, vec_splat(v.v, 0)), _mm_mul_ps(c[1].v, vec_splat(v.v, 1))), _mm_mul_ps(c[2].v, vec_splat(v.v, 2)))); } inline sse_meta_f12 operator * (const sse_meta_f12& mat) const { return sse_meta_f12((*this) * mat.c[0], (*this) * mat.c[1], (*this) * mat.c[2]); } inline sse_meta_f12 operator + (const sse_meta_f12& mat) const { return sse_meta_f12(c[0] + mat.c[0], c[1] + mat.c[1], c[2] + mat.c[2]); } inline sse_meta_f12 operator - (const sse_meta_f12& mat) const { return sse_meta_f12(c[0] - mat.c[0], c[1] - mat.c[1], c[2] - mat.c[2]); } inline sse_meta_f12 operator + (float t_) const { sse_meta_f4 t(t_); return sse_meta_f12(c[0] + t, c[1] + t, c[2] + t); } inline sse_meta_f12 operator - (float t_) const { sse_meta_f4 t(t_); return sse_meta_f12(c[0] - t, c[1] - t, c[2] - t); } inline sse_meta_f12 operator * (float t_) const { sse_meta_f4 t(t_); return sse_meta_f12(c[0] * t, c[1] * t, c[2] * t); } inline sse_meta_f12 operator / (float t_) const { sse_meta_f4 t(t_); return sse_meta_f12(c[0] / t, c[1] / t, c[2] / t); } inline sse_meta_f12& operator *= (const sse_meta_f12& mat) { setColumn((*this) * mat.c[0], (*this) * mat.c[1], (*this) * mat.c[2]); return *this; } inline sse_meta_f12& operator += (const sse_meta_f12& mat) { c[0] += mat.c[0]; c[1] += mat.c[1]; c[2] += mat.c[2]; return *this; } inline sse_meta_f12& operator -= (const sse_meta_f12& mat) { c[0] -= mat.c[0]; c[1] -= mat.c[1]; c[2] -= mat.c[2]; return *this; } inline sse_meta_f12& operator += (float t_) { sse_meta_f4 t(t_); c[0] += t; c[1] += t; c[2] += t; return *this; } inline sse_meta_f12& operator -= (float t_) { sse_meta_f4 t(t_); c[0] -= t; c[1] -= t; c[2] -= t; return *this; } inline sse_meta_f12& operator *= (float t_) { sse_meta_f4 t(t_); c[0] *= t; c[1] *= t; c[2] *= t; return *this; } inline sse_meta_f12& operator /= (float t_) { sse_meta_f4 t(t_); c[0] /= t; c[1] /= t; c[2] /= t; return *this; } inline sse_meta_f12& inverse() { __m128 inv0, inv1, inv2; details::inverse(c[0].v, c[1].v, c[2].v, &inv0, &inv1, &inv2); setColumn(inv0, inv1, inv2); return *this; } inline sse_meta_f12& transpose() { __m128 r0, r1, r2; details::transpose(c[0].v, c[1].v, c[2].v, &r0, &r1, &r2); setColumn(r0, r1, r2); return *this; } inline sse_meta_f12& abs() { c[0] = details::abs(c[0]); c[1] = details::abs(c[1]); c[2] = details::abs(c[2]); return *this; } inline float determinant() const { return _mm_cvtss_f32(dot_prod3(c[2].v, cross_prod(c[0].v, c[1].v))); } inline sse_meta_f12 transposeTimes(const sse_meta_f12& other) const { return sse_meta_f12(dot_prod3(c[0], other.c[0]), dot_prod3(c[0], other.c[1]), dot_prod3(c[0], other.c[2]), dot_prod3(c[1], other.c[0]), dot_prod3(c[1], other.c[1]), dot_prod3(c[1], other.c[2]), dot_prod3(c[2], other.c[0]), dot_prod3(c[2], other.c[1]), dot_prod3(c[2], other.c[2])); } inline sse_meta_f12 timesTranspose(const sse_meta_f12& m) const { sse_meta_f12 tmp(m); return (*this) * tmp.transpose(); } inline sse_meta_f4 transposeTimes(const sse_meta_f4& v) const { return sse_meta_f4(dot_prod3(c[0], v), dot_prod3(c[1], v), dot_prod3(c[2], v)); } inline float transposeDot(size_t i, const sse_meta_f4& v) const { return dot_prod3(c[i], v); } inline float dot(size_t i, const sse_meta_f4& v) const { return v[0] * c[0][i] + v[1] * c[1][i] + v[2] * c[2][i]; } }; static inline sse_meta_f12 abs(const sse_meta_f12& mat) { return sse_meta_f12(abs(mat.getColumn(0)), abs(mat.getColumn(1)), abs(mat.getColumn(2))); } static inline sse_meta_f12 transpose(const sse_meta_f12& mat) { __m128 r0, r1, r2; transpose(mat.getColumn(0).v, mat.getColumn(1).v, mat.getColumn(2).v, &r0, &r1, &r2); return sse_meta_f12(r0, r1, r2); } static inline sse_meta_f12 inverse(const sse_meta_f12& mat) { __m128 inv0, inv1, inv2; inverse(mat.getColumn(0).v, mat.getColumn(1).v, mat.getColumn(2).v, &inv0, &inv1, &inv2); return sse_meta_f12(inv0, inv1, inv2); } static inline void transpose(__m128 c0, __m128 c1, __m128 c2, __m128 c3, __m128* r0, __m128* r1, __m128* r2, __m128* r3) { __m128 tmp0 = _mm_unpacklo_ps(c0, c2); __m128 tmp1 = _mm_unpacklo_ps(c1, c3); __m128 tmp2 = _mm_unpackhi_ps(c0, c2); __m128 tmp3 = _mm_unpackhi_ps(c1, c3); *r0 = _mm_unpacklo_ps(tmp0, tmp1); *r1 = _mm_unpackhi_ps(tmp0, tmp1); *r2 = _mm_unpacklo_ps(tmp2, tmp3); *r3 = _mm_unpackhi_ps(tmp2, tmp3); } static inline void inverse(__m128 c0, __m128 c1, __m128 c2, __m128 c3, __m128* res0, __m128* res1, __m128* res2, __m128* res3) { __m128 Va, Vb, Vc; __m128 r1, r2, r3, tt, tt2; __m128 sum, Det, RDet; __m128 trns0, trns1, trns2, trns3; // Calculating the minterms for the first line. tt = c3; tt2 = _mm_ror_ps(c2,1); Vc = _mm_mul_ps(tt2,_mm_ror_ps(tt,0)); // V3'\B7V4 Va = _mm_mul_ps(tt2,_mm_ror_ps(tt,2)); // V3'\B7V4" Vb = _mm_mul_ps(tt2,_mm_ror_ps(tt,3)); // V3'\B7V4^ r1 = _mm_sub_ps(_mm_ror_ps(Va,1),_mm_ror_ps(Vc,2)); // V3"\B7V4^ - V3^\B7V4" r2 = _mm_sub_ps(_mm_ror_ps(Vb,2),_mm_ror_ps(Vb,0)); // V3^\B7V4' - V3'\B7V4^ r3 = _mm_sub_ps(_mm_ror_ps(Va,0),_mm_ror_ps(Vc,1)); // V3'\B7V4" - V3"\B7V4' tt = c1; Va = _mm_ror_ps(tt,1); sum = _mm_mul_ps(Va,r1); Vb = _mm_ror_ps(tt,2); sum = _mm_add_ps(sum,_mm_mul_ps(Vb,r2)); Vc = _mm_ror_ps(tt,3); sum = _mm_add_ps(sum,_mm_mul_ps(Vc,r3)); // Calculating the determinant. Det = _mm_mul_ps(sum,c0); Det = _mm_add_ps(Det,_mm_movehl_ps(Det,Det)); static const union { int i[4]; __m128 m; } Sign_PNPN __attribute__ ((aligned(16))) = {{0x00000000, 0x80000000, 0x00000000, 0x80000000}}; static const union { int i[4]; __m128 m; } Sign_NPNP __attribute__ ((aligned(16))) = {{0x80000000, 0x00000000, 0x80000000, 0x00000000}}; static const union { float i[4]; __m128 m; } ZERONE __attribute__ ((aligned(16))) = {{1.0f, 0.0f, 0.0f, 1.0f}}; __m128 mtL1 = _mm_xor_ps(sum,Sign_PNPN.m); // Calculating the minterms of the second line (using previous results). tt = _mm_ror_ps(c0,1); sum = _mm_mul_ps(tt,r1); tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r2)); tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r3)); __m128 mtL2 = _mm_xor_ps(sum,Sign_NPNP.m); // Testing the determinant. Det = _mm_sub_ss(Det,_mm_shuffle_ps(Det,Det,1)); // Calculating the minterms of the third line. tt = _mm_ror_ps(c0,1); Va = _mm_mul_ps(tt,Vb); // V1'\B7V2" Vb = _mm_mul_ps(tt,Vc); // V1'\B7V2^ Vc = _mm_mul_ps(tt,c1); // V1'\B7V2 r1 = _mm_sub_ps(_mm_ror_ps(Va,1),_mm_ror_ps(Vc,2)); // V1"\B7V2^ - V1^\B7V2" r2 = _mm_sub_ps(_mm_ror_ps(Vb,2),_mm_ror_ps(Vb,0)); // V1^\B7V2' - V1'\B7V2^ r3 = _mm_sub_ps(_mm_ror_ps(Va,0),_mm_ror_ps(Vc,1)); // V1'\B7V2" - V1"\B7V2' tt = _mm_ror_ps(c3,1); sum = _mm_mul_ps(tt,r1); tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r2)); tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r3)); __m128 mtL3 = _mm_xor_ps(sum,Sign_PNPN.m); // Dividing is FASTER than rcp_nr! (Because rcp_nr causes many register-memory RWs). RDet = _mm_div_ss(ZERONE.m, Det); // TODO: just 1.0f? RDet = _mm_shuffle_ps(RDet,RDet,0x00); // Devide the first 12 minterms with the determinant. mtL1 = _mm_mul_ps(mtL1, RDet); mtL2 = _mm_mul_ps(mtL2, RDet); mtL3 = _mm_mul_ps(mtL3, RDet); // Calculate the minterms of the forth line and devide by the determinant. tt = _mm_ror_ps(c2,1); sum = _mm_mul_ps(tt,r1); tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r2)); tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r3)); __m128 mtL4 = _mm_xor_ps(sum,Sign_NPNP.m); mtL4 = _mm_mul_ps(mtL4, RDet); // Now we just have to transpose the minterms matrix. trns0 = _mm_unpacklo_ps(mtL1,mtL2); trns1 = _mm_unpacklo_ps(mtL3,mtL4); trns2 = _mm_unpackhi_ps(mtL1,mtL2); trns3 = _mm_unpackhi_ps(mtL3,mtL4); *res0 = _mm_movelh_ps(trns0,trns1); *res1 = _mm_movehl_ps(trns1,trns0); *res2 = _mm_movelh_ps(trns2,trns3); *res3 = _mm_movehl_ps(trns3,trns2); } struct sse_meta_f16 { typedef float meta_type; typedef sse_meta_f4 vector_type; sse_meta_f4 c[4]; sse_meta_f16() { setZero(); } sse_meta_f16(float xx, float xy, float xz, float xw, float yx, float yy, float yz, float yw, float zx, float zy, float zz, float zw, float wx, float wy, float wz, float ww) { setValue(xx, xy, xz, xw, yz, yy, yz, yw, zx, zy, zz, zw, wx, wy, wz, ww); } sse_meta_f16(const sse_meta_f4& x, const sse_meta_f4& y, const sse_meta_f4& z, const sse_meta_f4& w) { setColumn(x, y, z, w); } sse_meta_f16(__m128 x, __m128 y, __m128 z, __m128 w) { setColumn(x, y, z, w); } inline void setValue(float xx, float xy, float xz, float xw, float yx, float yy, float yz, float yw, float zx, float zy, float zz, float zw, float wx, float wy, float wz, float ww) { c[0].setValue(xx, yx, zx, wx); c[1].setValue(xy, yy, zy, wy); c[2].setValue(xz, yz, zz, wz); c[3].setValue(xw, yw, zw, ww); } inline void setColumn(const sse_meta_f4& x, const sse_meta_f4& y, const sse_meta_f4& z, const sse_meta_f4& w) { c[0] = x; c[1] = y; c[2] = z; c[3] = w; } inline void setColumn(__m128 x, __m128 y, __m128 z, __m128 w) { c[0].setValue(x); c[1].setValue(y); c[2].setValue(z); c[3].setValue(w); } inline void setIdentity() { c[0].setValue(1, 0, 0, 0); c[1].setValue(0, 1, 0, 0); c[2].setValue(0, 0, 1, 0); c[3].setValue(0, 0, 0, 1); } inline void setZero() { c[0].setValue(0); c[1].setValue(0); c[2].setValue(0); c[3].setValue(0); } inline const sse_meta_f4& getColumn(size_t i) const { return c[i]; } inline sse_meta_f4& getColumn(size_t i) { return c[i]; } inline sse_meta_f4 getRow(size_t i) const { return sse_meta_f4(c[0][i], c[1][i], c[2][i], c[3][i]); } inline float operator () (size_t i, size_t j) const { return c[j][i]; } inline float& operator () (size_t i, size_t j) { return c[j][i]; } inline sse_meta_f4 operator * (const sse_meta_f4& v) const { return sse_meta_f4(_mm_add_ps(_mm_add_ps(_mm_mul_ps(c[0].v, vec_splat(v.v, 0)), _mm_mul_ps(c[1].v, vec_splat(v.v, 1))), _mm_add_ps(_mm_mul_ps(c[2].v, vec_splat(v.v, 2)), _mm_mul_ps(c[3].v, vec_splat(v.v, 3))) )); } inline sse_meta_f16 operator * (const sse_meta_f16& mat) const { return sse_meta_f16((*this) * mat.c[0], (*this) * mat.c[1], (*this) * mat.c[2], (*this) * mat.c[3]); } inline sse_meta_f16 operator + (const sse_meta_f16& mat) const { return sse_meta_f16(c[0] + mat.c[0], c[1] + mat.c[1], c[2] + mat.c[2], c[3] + mat.c[3]); } inline sse_meta_f16 operator - (const sse_meta_f16& mat) const { return sse_meta_f16(c[0] - mat.c[0], c[1] - mat.c[1], c[2] - mat.c[2], c[3] - mat.c[3]); } inline sse_meta_f16 operator + (float t_) const { sse_meta_f4 t(t_); return sse_meta_f16(c[0] + t, c[1] + t, c[2] + t, c[3] + t); } inline sse_meta_f16 operator - (float t_) const { sse_meta_f4 t(t_); return sse_meta_f16(c[0] - t, c[1] - t, c[2] - t, c[3] - t); } inline sse_meta_f16 operator * (float t_) const { sse_meta_f4 t(t_); return sse_meta_f16(c[0] * t, c[1] * t, c[2] * t, c[3] * t); } inline sse_meta_f16 operator / (float t_) const { sse_meta_f4 t(t_); return sse_meta_f16(c[0] / t, c[1] / t, c[2] / t, c[3] / t); } inline sse_meta_f16& operator *= (const sse_meta_f16& mat) { setColumn((*this) * mat.c[0], (*this) * mat.c[1], (*this) * mat.c[2], (*this) * mat.c[3]); return *this; } inline sse_meta_f16& operator += (const sse_meta_f16& mat) { c[0] += mat.c[0]; c[1] += mat.c[1]; c[2] += mat.c[2]; c[3] += mat.c[3]; return *this; } inline sse_meta_f16& operator -= (const sse_meta_f16& mat) { c[0] -= mat.c[0]; c[1] -= mat.c[1]; c[2] -= mat.c[2]; c[3] -= mat.c[3]; return *this; } inline sse_meta_f16& operator += (float t_) { sse_meta_f4 t(t_); c[0] += t; c[1] += t; c[2] += t; c[3] += t; return *this; } inline sse_meta_f16& operator -= (float t_) { sse_meta_f4 t(t_); c[0] -= t; c[1] -= t; c[2] -= t; c[3] -= t; return *this; } inline sse_meta_f16& operator *= (float t_) { sse_meta_f4 t(t_); c[0] *= t; c[1] *= t; c[2] *= t; c[3] *= t; return *this; } inline sse_meta_f16& operator /= (float t_) { sse_meta_f4 t(t_); c[0] /= t; c[1] /= t; c[2] /= t; c[3] /= t; return *this; } inline sse_meta_f16& abs() { c[0] = details::abs(c[0]); c[1] = details::abs(c[1]); c[2] = details::abs(c[2]); c[3] = details::abs(c[3]); return *this; } inline sse_meta_f16& inverse() { __m128 r0, r1, r2, r3; details::inverse(c[0].v, c[1].v, c[2].v, c[3].v, &r0, &r1, &r2, &r3); setColumn(r0, r1, r2, r3); return *this; } inline sse_meta_f16& transpose() { __m128 r0, r1, r2, r3; details::transpose(c[0].v, c[1].v, c[2].v, c[3].v, &r0, &r1, &r2, &r3); setColumn(r0, r1, r2, r3); return *this; } inline float determinant() const { __m128 Va, Vb, Vc; __m128 r1, r2, r3, tt, tt2; __m128 sum, Det; __m128 _L1 = c[0].v; __m128 _L2 = c[1].v; __m128 _L3 = c[2].v; __m128 _L4 = c[3].v; // Calculating the minterms for the first line. // _mm_ror_ps is just a macro using _mm_shuffle_ps(). tt = _L4; tt2 = _mm_ror_ps(_L3,1); Vc = _mm_mul_ps(tt2,_mm_ror_ps(tt,0)); // V3'·V4 Va = _mm_mul_ps(tt2,_mm_ror_ps(tt,2)); // V3'·V4" Vb = _mm_mul_ps(tt2,_mm_ror_ps(tt,3)); // V3'·V4^ r1 = _mm_sub_ps(_mm_ror_ps(Va,1),_mm_ror_ps(Vc,2)); // V3"·V4^ - V3^·V4" r2 = _mm_sub_ps(_mm_ror_ps(Vb,2),_mm_ror_ps(Vb,0)); // V3^·V4' - V3'·V4^ r3 = _mm_sub_ps(_mm_ror_ps(Va,0),_mm_ror_ps(Vc,1)); // V3'·V4" - V3"·V4' tt = _L2; Va = _mm_ror_ps(tt,1); sum = _mm_mul_ps(Va,r1); Vb = _mm_ror_ps(tt,2); sum = _mm_add_ps(sum,_mm_mul_ps(Vb,r2)); Vc = _mm_ror_ps(tt,3); sum = _mm_add_ps(sum,_mm_mul_ps(Vc,r3)); // Calculating the determinant. Det = _mm_mul_ps(sum,_L1); Det = _mm_add_ps(Det,_mm_movehl_ps(Det,Det)); // Calculating the minterms of the second line (using previous results). tt = _mm_ror_ps(_L1,1); sum = _mm_mul_ps(tt,r1); tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r2)); tt = _mm_ror_ps(tt,1); sum = _mm_add_ps(sum,_mm_mul_ps(tt,r3)); // Testing the determinant. Det = _mm_sub_ss(Det,_mm_shuffle_ps(Det,Det,1)); return _mm_cvtss_f32(Det); } inline sse_meta_f16 transposeTimes(const sse_meta_f16& other) const { return sse_meta_f16(dot_prod4(c[0], other.c[0]), dot_prod4(c[0], other.c[1]), dot_prod4(c[0], other.c[2]), dot_prod4(c[0], other.c[3]), dot_prod4(c[1], other.c[0]), dot_prod4(c[1], other.c[1]), dot_prod4(c[1], other.c[2]), dot_prod4(c[1], other.c[3]), dot_prod4(c[2], other.c[0]), dot_prod4(c[2], other.c[1]), dot_prod4(c[2], other.c[2]), dot_prod4(c[2], other.c[3]), dot_prod4(c[3], other.c[0]), dot_prod4(c[3], other.c[1]), dot_prod4(c[3], other.c[2]), dot_prod4(c[3], other.c[3])); } inline sse_meta_f16 timesTranspose(const sse_meta_f16& m) const { sse_meta_f16 tmp(m); return (*this) * tmp.transpose(); } inline sse_meta_f4 transposeTimes(const sse_meta_f4& v) const { return sse_meta_f4(dot_prod4(c[0], v), dot_prod4(c[1], v), dot_prod4(c[2], v), dot_prod4(c[3], v)); } inline float transposeDot(size_t i, const sse_meta_f4& v) const { return dot_prod4(c[i], v); } inline float dot(size_t i, const sse_meta_f4& v) const { return v[0] * c[0][i] + v[1] * c[1][i] + v[2] * c[2][i] + v[3] * c[3][i]; } }; static inline sse_meta_f16 abs(const sse_meta_f16& mat) { return sse_meta_f16(abs(mat.getColumn(0)), abs(mat.getColumn(1)), abs(mat.getColumn(2)), abs(mat.getColumn(3))); } static inline sse_meta_f16 transpose(const sse_meta_f16& mat) { __m128 r0, r1, r2, r3; transpose(mat.getColumn(0).v, mat.getColumn(1).v, mat.getColumn(2).v, mat.getColumn(3).v, &r0, &r1, &r2, &r3); return sse_meta_f16(r0, r1, r2, r3); } static inline sse_meta_f16 inverse(const sse_meta_f16& mat) { __m128 r0, r1, r2, r3; inverse(mat.getColumn(0).v, mat.getColumn(1).v, mat.getColumn(2).v, mat.getColumn(3).v, &r0, &r1, &r2, &r3); return sse_meta_f16(r0, r1, r2, r3); } } // details } // fcl #endif fcl-0.5.0/include/fcl/simd/simd_intersect.h000066400000000000000000000267321274356571000206140ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_MULTIPLE_INTERSECT #define FCL_MULTIPLE_INTERSECT #include "fcl/math/vec_3f.h" #include #include namespace fcl { static inline __m128 sse_four_spheres_intersect(const Vec3f& o1, FCL_REAL r1, const Vec3f& o2, FCL_REAL r2, const Vec3f& o3, FCL_REAL r3, const Vec3f& o4, FCL_REAL r4, const Vec3f& o5, FCL_REAL r5, const Vec3f& o6, FCL_REAL r6, const Vec3f& o7, FCL_REAL r7, const Vec3f& o8, FCL_REAL r8) { __m128 PX, PY, PZ, PR, QX, QY, QZ, QR; PX = _mm_set_ps(o1[0], o2[0], o3[0], o4[0]); PY = _mm_set_ps(o1[1], o2[1], o3[1], o4[1]); PZ = _mm_set_ps(o1[2], o2[2], o3[2], o4[2]); PR = _mm_set_ps(r1, r2, r3, r4); QX = _mm_set_ps(o5[0], o6[0], o7[0], o8[0]); QY = _mm_set_ps(o5[1], o6[1], o7[1], o8[1]); QZ = _mm_set_ps(o5[2], o6[2], o7[2], o8[2]); QR = _mm_set_ps(r5, r6, r7, r8); __m128 T1 = _mm_sub_ps(PX, QX); __m128 T2 = _mm_sub_ps(PY, QY); __m128 T3 = _mm_sub_ps(PZ, QZ); __m128 T4 = _mm_add_ps(PR, QR); T1 = _mm_mul_ps(T1, T1); T2 = _mm_mul_ps(T2, T2); T3 = _mm_mul_ps(T3, T3); T4 = _mm_mul_ps(T4, T4); T1 = _mm_add_ps(T1, T2); T2 = _mm_sub_ps(R2, T3); return _mm_cmple_ps(T1, T2); } static inline __m128 sse_four_spheres_four_AABBs_intersect(const Vec3f& o1, FCL_REAL r1, const Vec3f& o2, FCL_REAL r2, const Vec3f& o3, FCL_REAL r3, const Vec3f& o4, FCL_REAL r4, const Vec3f& min1, const Vec3f& max1, const Vec3f& min2, const Vec3f& max2, const Vec3f& min3, const Vec3f& max3, const Vec3f& min4, const Vec3f& max4) { __m128 MINX, MINY, MINZ, MAXX, MAXX, MAXY, MAXZ, SX, SY, SZ, SR; MINX = _mm_set_ps(min1[0], min2[0], min3[0], min4[0]); MAXX = _mm_set_ps(max1[0], max2[0], max3[0], max4[0]); MINY = _mm_set_ps(min1[1], min2[1], min3[1], min4[1]); MAXY = _mm_set_ps(max1[1], max2[1], max3[1], max4[1]); MINZ = _mm_set_ps(min1[2], min2[2], min3[2], min4[2]); MAXZ = _mm_set_ps(max1[2], max2[2], max3[2], max4[2]); SX = _mm_set_ps(o1[0], o2[0], o3[0], o4[0]); SY = _mm_set_ps(o1[1], o2[1], o3[1], o4[1]); SZ = _mm_set_ps(o1[2], o2[2], o3[2], o4[2]); SR = _mm_set_ps(r1, r2, r3, r4); __m128 TX = _mm_max_ps(SX, MINX); __m128 TY = _mm_max_ps(SY, MINY); __m128 TZ = _mm_max_ps(SZ, MINZ); TX = _mm_min_ps(TX, MAXX); TY = _mm_min_ps(TY, MAXY); TZ = _mm_min_ps(TZ, MAXZ); __m128 DX = _mm_sub_ps(SX, TX); __m128 DY = _mm_sub_ps(SY, TY); __m128 DZ = _mm_sub_ps(SZ, TZ); __m128 R2 = _mm_mul_ps(SR, SR); DX = _mm_mul_ps(DX, DX); DY = _mm_mul_ps(DY, DY); DZ = _mm_mul_ps(DZ, DZ); __m128 T1 = _mm_add_ps(DX, DY); __m128 T2 = _mm_sub_ps(R2, DZ); return _mm_cmple_ps(T1, T2); } static inline __m128 sse_four_AABBs_intersect(const Vec3f& min1, const Vec3f& max1, const Vec3f& min2, const Vec3f& max2, const Vec3f& min3, const Vec3f& max3, const Vec3f& min4, const Vec3f& max4, const Vec3f& min5, const Vec3f& max5, const Vec3f& min6, const Vec3f& max6, const Vec3f& min7, const Vec3f& max7, const Vec3f& min8, const Vec3f& max8) { __m128 MIN1X, MIN1Y, MIN1Z, MAX1X, MAX1Y, MAX1Z, MIN2X, MIN2Y, MIN2Z, MAX2X, MAX2Y, MAX2Z; MIN1X = _mm_set_ps(min1[0], min2[0], min3[0], min4[0]); MAX1X = _mm_set_ps(max1[0], max2[0], max3[0], max4[0]); MIN1Y = _mm_set_ps(min1[1], min2[1], min3[1], min4[1]); MAX1Y = _mm_set_ps(max1[1], max2[1], max3[1], max4[1]); MIN1Z = _mm_set_ps(min1[2], min2[2], min3[2], min4[2]); MAX1Z = _mm_set_ps(max1[2], max2[2], max3[2], max4[2]); MIN2X = _mm_set_ps(min5[0], min6[0], min7[0], min8[0]); MAX2X = _mm_set_ps(max5[0], max6[0], max7[0], max8[0]); MIN2Y = _mm_set_ps(min5[1], min6[1], min7[1], min8[1]); MAX2Y = _mm_set_ps(max5[1], max6[1], max7[1], max8[1]); MIN2Z = _mm_set_ps(min5[2], min6[2], min7[2], min8[2]); MAX2Z = _mm_set_ps(max5[2], max6[2], max7[2], max8[2]); __m128 AX = _mm_max_ps(MIN1X, MIN2X); __m128 BX = _mm_min_ps(MAX1X, MAX2X); __m128 AY = _mm_max_ps(MIN1Y, MIN2Y); __m128 BY = _mm_min_ps(MAX1Y, MAX2Y); __m128 AZ = _mm_max_ps(MIN1Z, MIN2Z); __m128 BZ = _mm_min_ps(MAX1Z, MAX2Z); __m128 T1 = _mm_cmple_ps(AX, BX); __m128 T2 = _mm_cmple_ps(AY, BY); __m128 T3 = _mm_cmple_ps(AZ, BZ); __m128 T4 = _mm_and_ps(T1, T2); return _mm_and_ps(T3, T4); } static bool four_spheres_intersect_and(const Vec3f& o1, FCL_REAL r1, const Vec3f& o2, FCL_REAL r2, const Vec3f& o3, FCL_REAL r3, const Vec3f& o4, FCL_REAL r4, const Vec3f& o5, FCL_REAL r5, const Vec3f& o6, FCL_REAL r6, const Vec3f& o7, FCL_REAL r7, const Vec3f& o8, FCL_REAL r8) { __m128 CMP = four_spheres_intersect(o1, r1, o2, r2, o3, r3, o4, r4, o5, r5, o6, r6, o7, r7, o8, r8); return _mm_movemask_ps(CMP) == 15.f; } static bool four_spheres_intersect_or(const Vec3f& o1, FCL_REAL r1, const Vec3f& o2, FCL_REAL r2, const Vec3f& o3, FCL_REAL r3, const Vec3f& o4, FCL_REAL r4, const Vec3f& o5, FCL_REAL r5, const Vec3f& o6, FCL_REAL r6, const Vec3f& o7, FCL_REAL r7, const Vec3f& o8, FCL_REAL r8) { __m128 CMP = four_spheres_intersect(o1, r1, o2, r2, o3, r3, o4, r4, o5, r5, o6, r6, o7, r7, o8, r8); return __mm_movemask_ps(CMP) > 0; } /** \brief four spheres versus four AABBs SIMD test */ static bool four_spheres_four_AABBs_intersect_and(const Vec3f& o1, FCL_REAL r1, const Vec3f& o2, FCL_REAL r2, const Vec3f& o3, FCL_REAL r3, const Vec3f& o4, FCL_REAL r4, const Vec3f& min1, const Vec3f& max1, const Vec3f& min2, const Vec3f& max2, const Vec3f& min3, const Vec3f& max3, const Vec3f& min4, const Vec3f& max4) { __m128 CMP = four_spheres_four_AABBs_intersect(o1, r1, o2, r2, o3, r3, o4, r4, min1, max1, min2, max2, min3, max3, min4, max4); return _mm_movemask_ps(CMP) == 15.f; } static bool four_spheres_four_AABBs_intersect_or(const Vec3f& o1, FCL_REAL r1, const Vec3f& o2, FCL_REAL r2, const Vec3f& o3, FCL_REAL r3, const Vec3f& o4, FCL_REAL r4, const Vec3f& min1, const Vec3f& max1, const Vec3f& min2, const Vec3f& max2, const Vec3f& min3, const Vec3f& max3, const Vec3f& min4, const Vec3f& max4) { __m128 CMP = four_spheres_four_AABBs_intersect(o1, r1, o2, r2, o3, r3, o4, r4, min1, max1, min2, max2, min3, max3, min4, max4); return _mm_movemask_ps(CMP) > 0; } /** \brief four AABBs versus four AABBs SIMD test */ static bool four_AABBs_intersect_and(const Vec3f& min1, const Vec3f& max1, const Vec3f& min2, const Vec3f& max2, const Vec3f& min3, const Vec3f& max3, const Vec3f& min4, const Vec3f& max4, const Vec3f& min5, const Vec3f& max5, const Vec3f& min6, const Vec3f& max6, const Vec3f& min7, const Vec3f& max7, const Vec3f& min8, const Vec3f& max8) { __m128 CMP = four_AABBs_intersect(min1, max1, min2, max2, min3, max3, min4, max4, min5, max5, min6, max6, min7, max7, min8, max8); return _mm_movemask_ps(CMP) == 15.f; } static bool four_AABBs_intersect_or(const Vec3f& min1, const Vec3f& max1, const Vec3f& min2, const Vec3f& max2, const Vec3f& min3, const Vec3f& max3, const Vec3f& min4, const Vec3f& max4, const Vec3f& min5, const Vec3f& max5, const Vec3f& min6, const Vec3f& max6, const Vec3f& min7, const Vec3f& max7, const Vec3f& min8, const Vec3f& max8) { __m128 CMP = four_AABBs_intersect(min1, max1, min2, max2, min3, max3, min4, max4, min5, max5, min6, max6, min7, max7, min8, max8); return _mm_movemask_ps(CMP) > 0; } } #endif fcl-0.5.0/include/fcl/traversal/000077500000000000000000000000001274356571000164645ustar00rootroot00000000000000fcl-0.5.0/include/fcl/traversal/traversal_node_base.h000066400000000000000000000123331274356571000226410ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_TRAVERSAL_NODE_BASE_H #define FCL_TRAVERSAL_NODE_BASE_H #include "fcl/data_types.h" #include "fcl/math/transform.h" #include "fcl/collision_data.h" namespace fcl { /// @brief Node structure encoding the information required for traversal. class TraversalNodeBase { public: virtual ~TraversalNodeBase(); virtual void preprocess() {} virtual void postprocess() {} /// @brief Whether b is a leaf node in the first BVH tree virtual bool isFirstNodeLeaf(int b) const; /// @brief Whether b is a leaf node in the second BVH tree virtual bool isSecondNodeLeaf(int b) const; /// @brief Traverse the subtree of the node in the first tree first virtual bool firstOverSecond(int b1, int b2) const; /// @brief Get the left child of the node b in the first tree virtual int getFirstLeftChild(int b) const; /// @brief Get the right child of the node b in the first tree virtual int getFirstRightChild(int b) const; /// @brief Get the left child of the node b in the second tree virtual int getSecondLeftChild(int b) const; /// @brief Get the right child of the node b in the second tree virtual int getSecondRightChild(int b) const; /// @brief Enable statistics (verbose mode) virtual void enableStatistics(bool enable) = 0; /// @brief configuation of first object Transform3f tf1; /// @brief configuration of second object Transform3f tf2; }; /// @brief Node structure encoding the information required for collision traversal. class CollisionTraversalNodeBase : public TraversalNodeBase { public: CollisionTraversalNodeBase() : result(NULL), enable_statistics(false) {} virtual ~CollisionTraversalNodeBase(); /// @brief BV test between b1 and b2 virtual bool BVTesting(int b1, int b2) const; /// @brief Leaf test between node b1 and b2, if they are both leafs virtual void leafTesting(int b1, int b2) const; /// @brief Check whether the traversal can stop virtual bool canStop() const; /// @brief Whether store some statistics information during traversal void enableStatistics(bool enable) { enable_statistics = enable; } /// @brief request setting for collision CollisionRequest request; /// @brief collision result kept during the traversal iteration CollisionResult* result; /// @brief Whether stores statistics bool enable_statistics; }; /// @brief Node structure encoding the information required for distance traversal. class DistanceTraversalNodeBase : public TraversalNodeBase { public: DistanceTraversalNodeBase() : result(NULL), enable_statistics(false) {} virtual ~DistanceTraversalNodeBase(); /// @brief BV test between b1 and b2 virtual FCL_REAL BVTesting(int b1, int b2) const; /// @brief Leaf test between node b1 and b2, if they are both leafs virtual void leafTesting(int b1, int b2) const; /// @brief Check whether the traversal can stop virtual bool canStop(FCL_REAL c) const; /// @brief Whether store some statistics information during traversal void enableStatistics(bool enable) { enable_statistics = enable; } /// @brief request setting for distance DistanceRequest request; /// @brief distance result kept during the traversal iteration DistanceResult* result; /// @brief Whether stores statistics bool enable_statistics; }; struct ConservativeAdvancementStackData { ConservativeAdvancementStackData(const Vec3f& P1_, const Vec3f& P2_, int c1_, int c2_, FCL_REAL d_) : P1(P1_), P2(P2_), c1(c1_), c2(c2_), d(d_) {} Vec3f P1; Vec3f P2; int c1; int c2; FCL_REAL d; }; } #endif fcl-0.5.0/include/fcl/traversal/traversal_node_bvh_shape.h000066400000000000000000001575461274356571000237060ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_TRAVERSAL_NODE_MESH_SHAPE_H #define FCL_TRAVERSAL_NODE_MESH_SHAPE_H #include "fcl/collision_data.h" #include "fcl/shape/geometric_shapes.h" #include "fcl/shape/geometric_shapes_utility.h" #include "fcl/traversal/traversal_node_base.h" #include "fcl/BVH/BVH_model.h" namespace fcl { /// @brief Traversal node for collision between BVH and shape template class BVHShapeCollisionTraversalNode : public CollisionTraversalNodeBase { public: BVHShapeCollisionTraversalNode() : CollisionTraversalNodeBase() { model1 = NULL; model2 = NULL; num_bv_tests = 0; num_leaf_tests = 0; query_time_seconds = 0.0; } /// @brief Whether the BV node in the first BVH tree is leaf bool isFirstNodeLeaf(int b) const { return model1->getBV(b).isLeaf(); } /// @brief Obtain the left child of BV node in the first BVH int getFirstLeftChild(int b) const { return model1->getBV(b).leftChild(); } /// @brief Obtain the right child of BV node in the first BVH int getFirstRightChild(int b) const { return model1->getBV(b).rightChild(); } /// @brief BV culling test in one BVTT node bool BVTesting(int b1, int b2) const { if(this->enable_statistics) num_bv_tests++; return !model1->getBV(b1).bv.overlap(model2_bv); } const BVHModel* model1; const S* model2; BV model2_bv; mutable int num_bv_tests; mutable int num_leaf_tests; mutable FCL_REAL query_time_seconds; }; /// @brief Traversal node for collision between shape and BVH template class ShapeBVHCollisionTraversalNode : public CollisionTraversalNodeBase { public: ShapeBVHCollisionTraversalNode() : CollisionTraversalNodeBase() { model1 = NULL; model2 = NULL; num_bv_tests = 0; num_leaf_tests = 0; query_time_seconds = 0.0; } /// @brief Alway extend the second model, which is a BVH model bool firstOverSecond(int, int) const { return false; } /// @brief Whether the BV node in the second BVH tree is leaf bool isSecondNodeLeaf(int b) const { return model2->getBV(b).isLeaf(); } /// @brief Obtain the left child of BV node in the second BVH int getSecondLeftChild(int b) const { return model2->getBV(b).leftChild(); } /// @brief Obtain the right child of BV node in the second BVH int getSecondRightChild(int b) const { return model2->getBV(b).rightChild(); } /// @brief BV culling test in one BVTT node bool BVTesting(int b1, int b2) const { if(this->enable_statistics) num_bv_tests++; return !model2->getBV(b2).bv.overlap(model1_bv); } const S* model1; const BVHModel* model2; BV model1_bv; mutable int num_bv_tests; mutable int num_leaf_tests; mutable FCL_REAL query_time_seconds; }; /// @brief Traversal node for collision between mesh and shape template class MeshShapeCollisionTraversalNode : public BVHShapeCollisionTraversalNode { public: MeshShapeCollisionTraversalNode() : BVHShapeCollisionTraversalNode() { vertices = NULL; tri_indices = NULL; nsolver = NULL; } /// @brief Intersection testing between leaves (one triangle and one shape) void leafTesting(int b1, int b2) const { if(this->enable_statistics) this->num_leaf_tests++; const BVNode& node = this->model1->getBV(b1); int primitive_id = node.primitiveId(); const Triangle& tri_id = tri_indices[primitive_id]; const Vec3f& p1 = vertices[tri_id[0]]; const Vec3f& p2 = vertices[tri_id[1]]; const Vec3f& p3 = vertices[tri_id[2]]; if(this->model1->isOccupied() && this->model2->isOccupied()) { bool is_intersect = false; if(!this->request.enable_contact) { if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, NULL, NULL, NULL)) { is_intersect = true; if(this->request.num_max_contacts > this->result->numContacts()) this->result->addContact(Contact(this->model1, this->model2, primitive_id, Contact::NONE)); } } else { FCL_REAL penetration; Vec3f normal; Vec3f contactp; if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, &contactp, &penetration, &normal)) { is_intersect = true; if(this->request.num_max_contacts > this->result->numContacts()) this->result->addContact(Contact(this->model1, this->model2, primitive_id, Contact::NONE, contactp, -normal, penetration)); } } if(is_intersect && this->request.enable_cost) { AABB overlap_part; AABB shape_aabb; computeBV(*(this->model2), this->tf2, shape_aabb); AABB(p1, p2, p3).overlap(shape_aabb, overlap_part); this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources); } } if((!this->model1->isFree() && !this->model2->isFree()) && this->request.enable_cost) { if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, NULL, NULL, NULL)) { AABB overlap_part; AABB shape_aabb; computeBV(*(this->model2), this->tf2, shape_aabb); AABB(p1, p2, p3).overlap(shape_aabb, overlap_part); this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources); } } } /// @brief Whether the traversal process can stop early bool canStop() const { return this->request.isSatisfied(*(this->result)); } Vec3f* vertices; Triangle* tri_indices; FCL_REAL cost_density; const NarrowPhaseSolver* nsolver; }; /// @cond IGNORE namespace details { template static inline void meshShapeCollisionOrientedNodeLeafTesting(int b1, int b2, const BVHModel* model1, const S& model2, Vec3f* vertices, Triangle* tri_indices, const Transform3f& tf1, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, bool enable_statistics, FCL_REAL cost_density, int& num_leaf_tests, const CollisionRequest& request, CollisionResult& result) { if(enable_statistics) num_leaf_tests++; const BVNode& node = model1->getBV(b1); int primitive_id = node.primitiveId(); const Triangle& tri_id = tri_indices[primitive_id]; const Vec3f& p1 = vertices[tri_id[0]]; const Vec3f& p2 = vertices[tri_id[1]]; const Vec3f& p3 = vertices[tri_id[2]]; if(model1->isOccupied() && model2.isOccupied()) { bool is_intersect = false; if(!request.enable_contact) // only interested in collision or not { if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, tf1, NULL, NULL, NULL)) { is_intersect = true; if(request.num_max_contacts > result.numContacts()) result.addContact(Contact(model1, &model2, primitive_id, Contact::NONE)); } } else { FCL_REAL penetration; Vec3f normal; Vec3f contactp; if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, tf1, &contactp, &penetration, &normal)) { is_intersect = true; if(request.num_max_contacts > result.numContacts()) result.addContact(Contact(model1, &model2, primitive_id, Contact::NONE, contactp, -normal, penetration)); } } if(is_intersect && request.enable_cost) { AABB overlap_part; AABB shape_aabb; computeBV(model2, tf2, shape_aabb); /* bool res = */ AABB(tf1.transform(p1), tf1.transform(p2), tf1.transform(p3)).overlap(shape_aabb, overlap_part); result.addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources); } } else if((!model1->isFree() || model2.isFree()) && request.enable_cost) { if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, tf1, NULL, NULL, NULL)) { AABB overlap_part; AABB shape_aabb; computeBV(model2, tf2, shape_aabb); /* bool res = */ AABB(tf1.transform(p1), tf1.transform(p2), tf1.transform(p3)).overlap(shape_aabb, overlap_part); result.addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources); } } } } /// @endcond /// @brief Traversal node for mesh and shape, when mesh BVH is one of the oriented node (OBB, RSS, OBBRSS, kIOS) template class MeshShapeCollisionTraversalNodeOBB : public MeshShapeCollisionTraversalNode { public: MeshShapeCollisionTraversalNodeOBB() : MeshShapeCollisionTraversalNode() { } bool BVTesting(int b1, int b2) const { if(this->enable_statistics) this->num_bv_tests++; return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv); } void leafTesting(int b1, int b2) const { details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices, this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->result)); } }; template class MeshShapeCollisionTraversalNodeRSS : public MeshShapeCollisionTraversalNode { public: MeshShapeCollisionTraversalNodeRSS() : MeshShapeCollisionTraversalNode() { } bool BVTesting(int b1, int b2) const { if(this->enable_statistics) this->num_bv_tests++; return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv); } void leafTesting(int b1, int b2) const { details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices, this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->result)); } }; template class MeshShapeCollisionTraversalNodekIOS : public MeshShapeCollisionTraversalNode { public: MeshShapeCollisionTraversalNodekIOS() : MeshShapeCollisionTraversalNode() { } bool BVTesting(int b1, int b2) const { if(this->enable_statistics) this->num_bv_tests++; return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv); } void leafTesting(int b1, int b2) const { details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices, this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->result)); } }; template class MeshShapeCollisionTraversalNodeOBBRSS : public MeshShapeCollisionTraversalNode { public: MeshShapeCollisionTraversalNodeOBBRSS() : MeshShapeCollisionTraversalNode() { } bool BVTesting(int b1, int b2) const { if(this->enable_statistics) this->num_bv_tests++; return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv); } void leafTesting(int b1, int b2) const { details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices, this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->result)); } }; /// @brief Traversal node for collision between shape and mesh template class ShapeMeshCollisionTraversalNode : public ShapeBVHCollisionTraversalNode { public: ShapeMeshCollisionTraversalNode() : ShapeBVHCollisionTraversalNode() { vertices = NULL; tri_indices = NULL; nsolver = NULL; } /// @brief Intersection testing between leaves (one shape and one triangle) void leafTesting(int b1, int b2) const { if(this->enable_statistics) this->num_leaf_tests++; const BVNode& node = this->model2->getBV(b2); int primitive_id = node.primitiveId(); const Triangle& tri_id = tri_indices[primitive_id]; const Vec3f& p1 = vertices[tri_id[0]]; const Vec3f& p2 = vertices[tri_id[1]]; const Vec3f& p3 = vertices[tri_id[2]]; if(this->model1->isOccupied() && this->model2->isOccupied()) { bool is_intersect = false; if(!this->request.enable_contact) { if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, NULL, NULL, NULL)) { is_intersect = true; if(this->request.num_max_contacts > this->result->numContacts()) this->result->addContact(Contact(this->model1, this->model2, Contact::NONE, primitive_id)); } } else { FCL_REAL penetration; Vec3f normal; Vec3f contactp; if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, &contactp, &penetration, &normal)) { is_intersect = true; if(this->request.num_max_contacts > this->result->numContacts()) this->result->addContact(Contact(this->model1, this->model2, Contact::NONE, primitive_id, contactp, normal, penetration)); } } if(is_intersect && this->request.enable_cost) { AABB overlap_part; AABB shape_aabb; computeBV(*(this->model1), this->tf1, shape_aabb); AABB(p1, p2, p3).overlap(shape_aabb, overlap_part); this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources); } } else if((!this->model1->isFree() && !this->model2->isFree()) && this->request.enable_cost) { if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, NULL, NULL, NULL)) { AABB overlap_part; AABB shape_aabb; computeBV(*(this->model1), this->tf1, shape_aabb); AABB(p1, p2, p3).overlap(shape_aabb, overlap_part); this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources); } } } /// @brief Whether the traversal process can stop early bool canStop() const { return this->request.isSatisfied(*(this->result)); } Vec3f* vertices; Triangle* tri_indices; FCL_REAL cost_density; const NarrowPhaseSolver* nsolver; }; /// @brief Traversal node for shape and mesh, when mesh BVH is one of the oriented node (OBB, RSS, OBBRSS, kIOS) template class ShapeMeshCollisionTraversalNodeOBB : public ShapeMeshCollisionTraversalNode { public: ShapeMeshCollisionTraversalNodeOBB() : ShapeMeshCollisionTraversalNode() { } bool BVTesting(int b1, int b2) const { if(this->enable_statistics) this->num_bv_tests++; return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv); } void leafTesting(int b1, int b2) const { details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices, this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request)); // may need to change the order in pairs } }; template class ShapeMeshCollisionTraversalNodeRSS : public ShapeMeshCollisionTraversalNode { public: ShapeMeshCollisionTraversalNodeRSS() : ShapeMeshCollisionTraversalNode() { } bool BVTesting(int b1, int b2) const { if(this->enable_statistics) this->num_bv_tests++; return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv); } void leafTesting(int b1, int b2) const { details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices, this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request)); // may need to change the order in pairs } }; template class ShapeMeshCollisionTraversalNodekIOS : public ShapeMeshCollisionTraversalNode { public: ShapeMeshCollisionTraversalNodekIOS() : ShapeMeshCollisionTraversalNode() { } bool BVTesting(int b1, int b2) const { if(this->enable_statistics) this->num_bv_tests++; return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv); } void leafTesting(int b1, int b2) const { details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices, this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request)); // may need to change the order in pairs } }; template class ShapeMeshCollisionTraversalNodeOBBRSS : public ShapeMeshCollisionTraversalNode { public: ShapeMeshCollisionTraversalNodeOBBRSS() : ShapeMeshCollisionTraversalNode() { } bool BVTesting(int b1, int b2) const { if(this->enable_statistics) this->num_bv_tests++; return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv); } void leafTesting(int b1, int b2) const { details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices, this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request)); // may need to change the order in pairs } }; /// @brief Traversal node for distance computation between BVH and shape template class BVHShapeDistanceTraversalNode : public DistanceTraversalNodeBase { public: BVHShapeDistanceTraversalNode() : DistanceTraversalNodeBase() { model1 = NULL; model2 = NULL; num_bv_tests = 0; num_leaf_tests = 0; query_time_seconds = 0.0; } /// @brief Whether the BV node in the first BVH tree is leaf bool isFirstNodeLeaf(int b) const { return model1->getBV(b).isLeaf(); } /// @brief Obtain the left child of BV node in the first BVH int getFirstLeftChild(int b) const { return model1->getBV(b).leftChild(); } /// @brief Obtain the right child of BV node in the first BVH int getFirstRightChild(int b) const { return model1->getBV(b).rightChild(); } /// @brief BV culling test in one BVTT node FCL_REAL BVTesting(int b1, int b2) const { return model1->getBV(b1).bv.distance(model2_bv); } const BVHModel* model1; const S* model2; BV model2_bv; mutable int num_bv_tests; mutable int num_leaf_tests; mutable FCL_REAL query_time_seconds; }; /// @brief Traversal node for distance computation between shape and BVH template class ShapeBVHDistanceTraversalNode : public DistanceTraversalNodeBase { public: ShapeBVHDistanceTraversalNode() : DistanceTraversalNodeBase() { model1 = NULL; model2 = NULL; num_bv_tests = 0; num_leaf_tests = 0; query_time_seconds = 0.0; } /// @brief Whether the BV node in the second BVH tree is leaf bool isSecondNodeLeaf(int b) const { return model2->getBV(b).isLeaf(); } /// @brief Obtain the left child of BV node in the second BVH int getSecondLeftChild(int b) const { return model2->getBV(b).leftChild(); } /// @brief Obtain the right child of BV node in the second BVH int getSecondRightChild(int b) const { return model2->getBV(b).rightChild(); } /// @brief BV culling test in one BVTT node FCL_REAL BVTesting(int b1, int b2) const { return model1_bv.distance(model2->getBV(b2).bv); } const S* model1; const BVHModel* model2; BV model1_bv; mutable int num_bv_tests; mutable int num_leaf_tests; mutable FCL_REAL query_time_seconds; }; /// @brief Traversal node for distance between mesh and shape template class MeshShapeDistanceTraversalNode : public BVHShapeDistanceTraversalNode { public: MeshShapeDistanceTraversalNode() : BVHShapeDistanceTraversalNode() { vertices = NULL; tri_indices = NULL; rel_err = 0; abs_err = 0; nsolver = NULL; } /// @brief Distance testing between leaves (one triangle and one shape) void leafTesting(int b1, int b2) const { if(this->enable_statistics) this->num_leaf_tests++; const BVNode& node = this->model1->getBV(b1); int primitive_id = node.primitiveId(); const Triangle& tri_id = tri_indices[primitive_id]; const Vec3f& p1 = vertices[tri_id[0]]; const Vec3f& p2 = vertices[tri_id[1]]; const Vec3f& p3 = vertices[tri_id[2]]; FCL_REAL d; Vec3f closest_p1, closest_p2; nsolver->shapeTriangleDistance(*(this->model2), this->tf2, p1, p2, p3, &d, &closest_p2, &closest_p1); this->result->update(d, this->model1, this->model2, primitive_id, DistanceResult::NONE, closest_p1, closest_p2); } /// @brief Whether the traversal process can stop early bool canStop(FCL_REAL c) const { if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance)) return true; return false; } Vec3f* vertices; Triangle* tri_indices; FCL_REAL rel_err; FCL_REAL abs_err; const NarrowPhaseSolver* nsolver; }; /// @cond IGNORE namespace details { template void meshShapeDistanceOrientedNodeLeafTesting(int b1, int /* b2 */, const BVHModel* model1, const S& model2, Vec3f* vertices, Triangle* tri_indices, const Transform3f& tf1, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, bool enable_statistics, int & num_leaf_tests, const DistanceRequest& /* request */, DistanceResult& result) { if(enable_statistics) num_leaf_tests++; const BVNode& node = model1->getBV(b1); int primitive_id = node.primitiveId(); const Triangle& tri_id = tri_indices[primitive_id]; const Vec3f& p1 = vertices[tri_id[0]]; const Vec3f& p2 = vertices[tri_id[1]]; const Vec3f& p3 = vertices[tri_id[2]]; FCL_REAL distance; Vec3f closest_p1, closest_p2; nsolver->shapeTriangleDistance(model2, tf2, p1, p2, p3, tf1, &distance, &closest_p2, &closest_p1); result.update(distance, model1, &model2, primitive_id, DistanceResult::NONE, closest_p1, closest_p2); } template static inline void distancePreprocessOrientedNode(const BVHModel* model1, Vec3f* vertices, Triangle* tri_indices, int init_tri_id, const S& model2, const Transform3f& tf1, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const DistanceRequest& /* request */, DistanceResult& result) { const Triangle& init_tri = tri_indices[init_tri_id]; const Vec3f& p1 = vertices[init_tri[0]]; const Vec3f& p2 = vertices[init_tri[1]]; const Vec3f& p3 = vertices[init_tri[2]]; FCL_REAL distance; Vec3f closest_p1, closest_p2; nsolver->shapeTriangleDistance(model2, tf2, p1, p2, p3, tf1, &distance, &closest_p2, &closest_p1); result.update(distance, model1, &model2, init_tri_id, DistanceResult::NONE, closest_p1, closest_p2); } } /// @endcond /// @brief Traversal node for distance between mesh and shape, when mesh BVH is one of the oriented node (RSS, OBBRSS, kIOS) template class MeshShapeDistanceTraversalNodeRSS : public MeshShapeDistanceTraversalNode { public: MeshShapeDistanceTraversalNodeRSS() : MeshShapeDistanceTraversalNode() { } void preprocess() { details::distancePreprocessOrientedNode(this->model1, this->vertices, this->tri_indices, 0, *(this->model2), this->tf1, this->tf2, this->nsolver, this->request, *(this->result)); } void postprocess() { } FCL_REAL BVTesting(int b1, int b2) const { if(this->enable_statistics) this->num_bv_tests++; return distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv); } void leafTesting(int b1, int b2) const { details::meshShapeDistanceOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices, this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result)); } }; template class MeshShapeDistanceTraversalNodekIOS : public MeshShapeDistanceTraversalNode { public: MeshShapeDistanceTraversalNodekIOS() : MeshShapeDistanceTraversalNode() { } void preprocess() { details::distancePreprocessOrientedNode(this->model1, this->vertices, this->tri_indices, 0, *(this->model2), this->tf1, this->tf2, this->nsolver, this->request, *(this->result)); } void postprocess() { } FCL_REAL BVTesting(int b1, int b2) const { if(this->enable_statistics) this->num_bv_tests++; return distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv); } void leafTesting(int b1, int b2) const { details::meshShapeDistanceOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices, this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result)); } }; template class MeshShapeDistanceTraversalNodeOBBRSS : public MeshShapeDistanceTraversalNode { public: MeshShapeDistanceTraversalNodeOBBRSS() : MeshShapeDistanceTraversalNode() { } void preprocess() { details::distancePreprocessOrientedNode(this->model1, this->vertices, this->tri_indices, 0, *(this->model2), this->tf1, this->tf2, this->nsolver, this->request, *(this->result)); } void postprocess() { } FCL_REAL BVTesting(int b1, int b2) const { if(this->enable_statistics) this->num_bv_tests++; return distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv); } void leafTesting(int b1, int b2) const { details::meshShapeDistanceOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices, this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result)); } }; /// @brief Traversal node for distance between shape and mesh template class ShapeMeshDistanceTraversalNode : public ShapeBVHDistanceTraversalNode { public: ShapeMeshDistanceTraversalNode() : ShapeBVHDistanceTraversalNode() { vertices = NULL; tri_indices = NULL; rel_err = 0; abs_err = 0; nsolver = NULL; } /// @brief Distance testing between leaves (one shape and one triangle) void leafTesting(int b1, int b2) const { if(this->enable_statistics) this->num_leaf_tests++; const BVNode& node = this->model2->getBV(b2); int primitive_id = node.primitiveId(); const Triangle& tri_id = tri_indices[primitive_id]; const Vec3f& p1 = vertices[tri_id[0]]; const Vec3f& p2 = vertices[tri_id[1]]; const Vec3f& p3 = vertices[tri_id[2]]; FCL_REAL distance; Vec3f closest_p1, closest_p2; nsolver->shapeTriangleDistance(*(this->model1), this->tf1, p1, p2, p3, &distance, &closest_p1, &closest_p2); this->result->update(distance, this->model1, this->model2, DistanceResult::NONE, primitive_id, closest_p1, closest_p2); } /// @brief Whether the traversal process can stop early bool canStop(FCL_REAL c) const { if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance)) return true; return false; } Vec3f* vertices; Triangle* tri_indices; FCL_REAL rel_err; FCL_REAL abs_err; const NarrowPhaseSolver* nsolver; }; template class ShapeMeshDistanceTraversalNodeRSS : public ShapeMeshDistanceTraversalNode { public: ShapeMeshDistanceTraversalNodeRSS() : ShapeMeshDistanceTraversalNode() { } void preprocess() { details::distancePreprocessOrientedNode(this->model2, this->vertices, this->tri_indices, 0, *(this->model1), this->tf2, this->tf1, this->nsolver, this->request, *(this->result)); } void postprocess() { } FCL_REAL BVTesting(int b1, int b2) const { if(this->enable_statistics) this->num_bv_tests++; return distance(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv); } void leafTesting(int b1, int b2) const { details::meshShapeDistanceOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices, this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result)); } }; template class ShapeMeshDistanceTraversalNodekIOS : public ShapeMeshDistanceTraversalNode { public: ShapeMeshDistanceTraversalNodekIOS() : ShapeMeshDistanceTraversalNode() { } void preprocess() { details::distancePreprocessOrientedNode(this->model2, this->vertices, this->tri_indices, 0, *(this->model1), this->tf2, this->tf1, this->nsolver, *(this->result)); } void postprocess() { } FCL_REAL BVTesting(int b1, int b2) const { if(this->enable_statistics) this->num_bv_tests++; return distance(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv); } void leafTesting(int b1, int b2) const { details::meshShapeDistanceOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices, this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result)); } }; template class ShapeMeshDistanceTraversalNodeOBBRSS : public ShapeMeshDistanceTraversalNode { public: ShapeMeshDistanceTraversalNodeOBBRSS() : ShapeMeshDistanceTraversalNode() { } void preprocess() { details::distancePreprocessOrientedNode(this->model2, this->vertices, this->tri_indices, 0, *(this->model1), this->tf2, this->tf1, this->nsolver, *(this->result)); } void postprocess() { } FCL_REAL BVTesting(int b1, int b2) const { if(this->enable_statistics) this->num_bv_tests++; return distance(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv); } void leafTesting(int b1, int b2) const { details::meshShapeDistanceOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices, this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->request, *(this->result)); } }; /// @brief Traversal node for conservative advancement computation between BVH and shape template class MeshShapeConservativeAdvancementTraversalNode : public MeshShapeDistanceTraversalNode { public: MeshShapeConservativeAdvancementTraversalNode(FCL_REAL w_ = 1) : MeshShapeDistanceTraversalNode() { delta_t = 1; toc = 0; t_err = (FCL_REAL)0.0001; w = w_; motion1 = NULL; motion2 = NULL; } /// @brief BV culling test in one BVTT node FCL_REAL BVTesting(int b1, int b2) const { if(this->enable_statistics) this->num_bv_tests++; Vec3f P1, P2; FCL_REAL d = this->model2_bv.distance(this->model1->getBV(b1).bv, &P2, &P1); stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d)); return d; } /// @brief Conservative advancement testing between leaves (one triangle and one shape) void leafTesting(int b1, int b2) const { if(this->enable_statistics) this->num_leaf_tests++; const BVNode& node = this->model1->getBV(b1); int primitive_id = node.primitiveId(); const Triangle& tri_id = this->tri_indices[primitive_id]; const Vec3f& p1 = this->vertices[tri_id[0]]; const Vec3f& p2 = this->vertices[tri_id[1]]; const Vec3f& p3 = this->vertices[tri_id[2]]; FCL_REAL d; Vec3f P1, P2; this->nsolver->shapeTriangleDistance(*(this->model2), this->tf2, p1, p2, p3, &d, &P2, &P1); if(d < this->min_distance) { this->min_distance = d; closest_p1 = P1; closest_p2 = P2; last_tri_id = primitive_id; } Vec3f n = this->tf2.transform(P2) - P1; n.normalize(); // here n should be in global frame TriangleMotionBoundVisitor mb_visitor1(p1, p2, p3, n); TBVMotionBoundVisitor mb_visitor2(this->model2_bv, -n); FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1); FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2); FCL_REAL bound = bound1 + bound2; FCL_REAL cur_delta_t; if(bound <= d) cur_delta_t = 1; else cur_delta_t = d / bound; if(cur_delta_t < delta_t) delta_t = cur_delta_t; } /// @brief Whether the traversal process can stop early bool canStop(FCL_REAL c) const { if((c >= w * (this->min_distance - this->abs_err)) && (c * (1 + this->rel_err) >= w * this->min_distance)) { const ConservativeAdvancementStackData& data = stack.back(); Vec3f n = this->tf2.transform(data.P2) - data.P1; n.normalize(); int c1 = data.c1; TBVMotionBoundVisitor mb_visitor1(this->model1->getBV(c1).bv, n); TBVMotionBoundVisitor mb_visitor2(this->model2_bv, -n); FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1); FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2); FCL_REAL bound = bound1 + bound2; FCL_REAL cur_delta_t; if(bound < c) cur_delta_t = 1; else cur_delta_t = c / bound; if(cur_delta_t < delta_t) delta_t = cur_delta_t; stack.pop_back(); return true; } else { stack.pop_back(); return false; } } mutable FCL_REAL min_distance; mutable Vec3f closest_p1, closest_p2; mutable int last_tri_id; /// @brief CA controlling variable: early stop for the early iterations of CA FCL_REAL w; /// @brief The time from beginning point FCL_REAL toc; FCL_REAL t_err; /// @brief The delta_t each step mutable FCL_REAL delta_t; /// @brief Motions for the two objects in query const MotionBase* motion1; const MotionBase* motion2; mutable std::vector stack; }; template class ShapeMeshConservativeAdvancementTraversalNode : public ShapeMeshDistanceTraversalNode { public: ShapeMeshConservativeAdvancementTraversalNode(FCL_REAL w_ = 1) : ShapeMeshDistanceTraversalNode() { delta_t = 1; toc = 0; t_err = (FCL_REAL)0.0001; w = w_; motion1 = NULL; motion2 = NULL; } /// @brief BV culling test in one BVTT node FCL_REAL BVTesting(int b1, int b2) const { if(this->enable_statistics) this->num_bv_tests++; Vec3f P1, P2; FCL_REAL d = this->model1_bv.distance(this->model2->getBV(b2).bv, &P1, &P2); stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d)); return d; } /// @brief Conservative advancement testing between leaves (one triangle and one shape) void leafTesting(int b1, int b2) const { if(this->enable_statistics) this->num_leaf_tests++; const BVNode& node = this->model2->getBV(b2); int primitive_id = node.primitiveId(); const Triangle& tri_id = this->tri_indices[primitive_id]; const Vec3f& p1 = this->vertices[tri_id[0]]; const Vec3f& p2 = this->vertices[tri_id[1]]; const Vec3f& p3 = this->vertices[tri_id[2]]; FCL_REAL d; Vec3f P1, P2; this->nsolver->shapeTriangleDistance(*(this->model1), this->tf1, p1, p2, p3, &d, &P1, &P2); if(d < this->min_distance) { this->min_distance = d; closest_p1 = P1; closest_p2 = P2; last_tri_id = primitive_id; } Vec3f n = P2 - this->tf1.transform(P1); n.normalize(); // here n should be in global frame TBVMotionBoundVisitor mb_visitor1(this->model1_bv, n); TriangleMotionBoundVisitor mb_visitor2(p1, p2, p3, -n); FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1); FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2); FCL_REAL bound = bound1 + bound2; FCL_REAL cur_delta_t; if(bound <= d) cur_delta_t = 1; else cur_delta_t = d / bound; if(cur_delta_t < delta_t) delta_t = cur_delta_t; } /// @brief Whether the traversal process can stop early bool canStop(FCL_REAL c) const { if((c >= w * (this->min_distance - this->abs_err)) && (c * (1 + this->rel_err) >= w * this->min_distance)) { const ConservativeAdvancementStackData& data = stack.back(); Vec3f n = data.P2 - this->tf1.transform(data.P1); n.normalize(); int c2 = data.c2; TBVMotionBoundVisitor mb_visitor1(this->model1_bv, n); TBVMotionBoundVisitor mb_visitor2(this->model2->getBV(c2).bv, -n); FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1); FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2); FCL_REAL bound = bound1 + bound2; FCL_REAL cur_delta_t; if(bound < c) cur_delta_t = 1; else cur_delta_t = c / bound; if(cur_delta_t < delta_t) delta_t = cur_delta_t; stack.pop_back(); return true; } else { stack.pop_back(); return false; } } mutable FCL_REAL min_distance; mutable Vec3f closest_p1, closest_p2; mutable int last_tri_id; /// @brief CA controlling variable: early stop for the early iterations of CA FCL_REAL w; /// @brief The time from beginning point FCL_REAL toc; FCL_REAL t_err; /// @brief The delta_t each step mutable FCL_REAL delta_t; /// @brief Motions for the two objects in query const MotionBase* motion1; const MotionBase* motion2; mutable std::vector stack; }; namespace details { template void meshShapeConservativeAdvancementOrientedNodeLeafTesting(int b1, int /* b2 */, const BVHModel* model1, const S& model2, const BV& model2_bv, Vec3f* vertices, Triangle* tri_indices, const Transform3f& tf1, const Transform3f& tf2, const MotionBase* motion1, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, bool enable_statistics, FCL_REAL& min_distance, Vec3f& p1, Vec3f& p2, int& last_tri_id, FCL_REAL& delta_t, int& num_leaf_tests) { if(enable_statistics) num_leaf_tests++; const BVNode& node = model1->getBV(b1); int primitive_id = node.primitiveId(); const Triangle& tri_id = tri_indices[primitive_id]; const Vec3f& t1 = vertices[tri_id[0]]; const Vec3f& t2 = vertices[tri_id[1]]; const Vec3f& t3 = vertices[tri_id[2]]; FCL_REAL distance; Vec3f P1, P2; nsolver->shapeTriangleDistance(model2, tf2, t1, t2, t3, tf1, &distance, &P2, &P1); if(distance < min_distance) { min_distance = distance; p1 = P1; p2 = P2; last_tri_id = primitive_id; } // n is in global frame Vec3f n = P2 - P1; n.normalize(); TriangleMotionBoundVisitor mb_visitor1(t1, t2, t3, n); TBVMotionBoundVisitor mb_visitor2(model2_bv, -n); FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1); FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2); FCL_REAL bound = bound1 + bound2; FCL_REAL cur_delta_t; if(bound <= distance) cur_delta_t = 1; else cur_delta_t = distance / bound; if(cur_delta_t < delta_t) delta_t = cur_delta_t; } template bool meshShapeConservativeAdvancementOrientedNodeCanStop(FCL_REAL c, FCL_REAL min_distance, FCL_REAL abs_err, FCL_REAL rel_err, FCL_REAL w, const BVHModel* model1, const S& model2, const BV& model2_bv, const MotionBase* motion1, const MotionBase* motion2, std::vector& stack, FCL_REAL& delta_t) { if((c >= w * (min_distance - abs_err)) && (c * (1 + rel_err) >= w * min_distance)) { const ConservativeAdvancementStackData& data = stack.back(); Vec3f n = data.P2 - data.P1; n.normalize(); int c1 = data.c1; TBVMotionBoundVisitor mb_visitor1(model1->getBV(c1).bv, n); TBVMotionBoundVisitor mb_visitor2(model2_bv, -n); FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1); FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2); FCL_REAL bound = bound1 + bound2; FCL_REAL cur_delta_t; if(bound <= c) cur_delta_t = 1; else cur_delta_t = c / bound; if(cur_delta_t < delta_t) delta_t = cur_delta_t; stack.pop_back(); return true; } else { stack.pop_back(); return false; } } } template class MeshShapeConservativeAdvancementTraversalNodeRSS : public MeshShapeConservativeAdvancementTraversalNode { public: MeshShapeConservativeAdvancementTraversalNodeRSS(FCL_REAL w_ = 1) : MeshShapeConservativeAdvancementTraversalNode(w_) { } FCL_REAL BVTesting(int b1, int b2) const { if(this->enable_statistics) this->num_bv_tests++; Vec3f P1, P2; FCL_REAL d = distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model1->getBV(b1).bv, this->model2_bv, &P1, &P2); this->stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d)); return d; } void leafTesting(int b1, int b2) const { details::meshShapeConservativeAdvancementOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->model2_bv, this->vertices, this->tri_indices, this->tf1, this->tf2, this->motion1, this->motion2, this->nsolver, this->enable_statistics, this->min_distance, this->closest_p1, this->closest_p2, this->last_tri_id, this->delta_t, this->num_leaf_tests); } bool canStop(FCL_REAL c) const { return details::meshShapeConservativeAdvancementOrientedNodeCanStop(c, this->min_distance, this->abs_err, this->rel_err, this->w, this->model1, *(this->model2), this->model2_bv, this->motion1, this->motion2, this->stack, this->delta_t); } }; template class MeshShapeConservativeAdvancementTraversalNodeOBBRSS : public MeshShapeConservativeAdvancementTraversalNode { public: MeshShapeConservativeAdvancementTraversalNodeOBBRSS(FCL_REAL w_ = 1) : MeshShapeConservativeAdvancementTraversalNode(w_) { } FCL_REAL BVTesting(int b1, int b2) const { if(this->enable_statistics) this->num_bv_tests++; Vec3f P1, P2; FCL_REAL d = distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model1->getBV(b1).bv, this->model2_bv, &P1, &P2); this->stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d)); return d; } void leafTesting(int b1, int b2) const { details::meshShapeConservativeAdvancementOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->model2_bv, this->vertices, this->tri_indices, this->tf1, this->tf2, this->motion1, this->motion2, this->nsolver, this->enable_statistics, this->min_distance, this->closest_p1, this->closest_p2, this->last_tri_id, this->delta_t, this->num_leaf_tests); } bool canStop(FCL_REAL c) const { return details::meshShapeConservativeAdvancementOrientedNodeCanStop(c, this->min_distance, this->abs_err, this->rel_err, this->w, this->model1, *(this->model2), this->model2_bv, this->motion1, this->motion2, this->stack, this->delta_t); } }; template class ShapeMeshConservativeAdvancementTraversalNodeRSS : public ShapeMeshConservativeAdvancementTraversalNode { public: ShapeMeshConservativeAdvancementTraversalNodeRSS(FCL_REAL w_ = 1) : ShapeMeshConservativeAdvancementTraversalNode(w_) { } FCL_REAL BVTesting(int b1, int b2) const { if(this->enable_statistics) this->num_bv_tests++; Vec3f P1, P2; FCL_REAL d = distance(this->tf2.getRotation(), this->tf2.getTranslation(), this->model2->getBV(b2).bv, this->model1_bv, &P2, &P1); this->stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d)); return d; } void leafTesting(int b1, int b2) const { details::meshShapeConservativeAdvancementOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1), this->model1_bv, this->vertices, this->tri_indices, this->tf2, this->tf1, this->motion2, this->motion1, this->nsolver, this->enable_statistics, this->min_distance, this->closest_p2, this->closest_p1, this->last_tri_id, this->delta_t, this->num_leaf_tests); } bool canStop(FCL_REAL c) const { return details::meshShapeConservativeAdvancementOrientedNodeCanStop(c, this->min_distance, this->abs_err, this->rel_err, this->w, this->model2, *(this->model1), this->model1_bv, this->motion2, this->motion1, this->stack, this->delta_t); } }; template class ShapeMeshConservativeAdvancementTraversalNodeOBBRSS : public ShapeMeshConservativeAdvancementTraversalNode { public: ShapeMeshConservativeAdvancementTraversalNodeOBBRSS(FCL_REAL w_ = 1) : ShapeMeshConservativeAdvancementTraversalNode(w_) { } FCL_REAL BVTesting(int b1, int b2) const { if(this->enable_statistics) this->num_bv_tests++; Vec3f P1, P2; FCL_REAL d = distance(this->tf2.getRotation(), this->tf2.getTranslation(), this->model2->getBV(b2).bv, this->model1_bv, &P2, &P1); this->stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d)); return d; } void leafTesting(int b1, int b2) const { details::meshShapeConservativeAdvancementOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1), this->model1_bv, this->vertices, this->tri_indices, this->tf2, this->tf1, this->motion2, this->motion1, this->nsolver, this->enable_statistics, this->min_distance, this->closest_p2, this->closest_p1, this->last_tri_id, this->delta_t, this->num_leaf_tests); } bool canStop(FCL_REAL c) const { return details::meshShapeConservativeAdvancementOrientedNodeCanStop(c, this->min_distance, this->abs_err, this->rel_err, this->w, this->model2, *(this->model1), this->model1_bv, this->motion2, this->motion1, this->stack, this->delta_t); } }; } #endif fcl-0.5.0/include/fcl/traversal/traversal_node_bvhs.h000066400000000000000000000662501274356571000227000ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_TRAVERSAL_NODE_MESHES_H #define FCL_TRAVERSAL_NODE_MESHES_H #include "fcl/collision_data.h" #include "fcl/traversal/traversal_node_base.h" #include "fcl/BV/BV_node.h" #include "fcl/BV/BV.h" #include "fcl/BVH/BVH_model.h" #include "fcl/intersect.h" #include "fcl/ccd/motion.h" #include #include #include #include namespace fcl { /// @brief Traversal node for collision between BVH models template class BVHCollisionTraversalNode : public CollisionTraversalNodeBase { public: BVHCollisionTraversalNode() : CollisionTraversalNodeBase() { model1 = NULL; model2 = NULL; num_bv_tests = 0; num_leaf_tests = 0; query_time_seconds = 0.0; } /// @brief Whether the BV node in the first BVH tree is leaf bool isFirstNodeLeaf(int b) const { return model1->getBV(b).isLeaf(); } /// @brief Whether the BV node in the second BVH tree is leaf bool isSecondNodeLeaf(int b) const { return model2->getBV(b).isLeaf(); } /// @brief Determine the traversal order, is the first BVTT subtree better bool firstOverSecond(int b1, int b2) const { FCL_REAL sz1 = model1->getBV(b1).bv.size(); FCL_REAL sz2 = model2->getBV(b2).bv.size(); bool l1 = model1->getBV(b1).isLeaf(); bool l2 = model2->getBV(b2).isLeaf(); if(l2 || (!l1 && (sz1 > sz2))) return true; return false; } /// @brief Obtain the left child of BV node in the first BVH int getFirstLeftChild(int b) const { return model1->getBV(b).leftChild(); } /// @brief Obtain the right child of BV node in the first BVH int getFirstRightChild(int b) const { return model1->getBV(b).rightChild(); } /// @brief Obtain the left child of BV node in the second BVH int getSecondLeftChild(int b) const { return model2->getBV(b).leftChild(); } /// @brief Obtain the right child of BV node in the second BVH int getSecondRightChild(int b) const { return model2->getBV(b).rightChild(); } /// @brief BV culling test in one BVTT node bool BVTesting(int b1, int b2) const { if(enable_statistics) num_bv_tests++; return !model1->getBV(b1).overlap(model2->getBV(b2)); } /// @brief The first BVH model const BVHModel* model1; /// @brief The second BVH model const BVHModel* model2; /// @brief statistical information mutable int num_bv_tests; mutable int num_leaf_tests; mutable FCL_REAL query_time_seconds; }; /// @brief Traversal node for collision between two meshes template class MeshCollisionTraversalNode : public BVHCollisionTraversalNode { public: MeshCollisionTraversalNode() : BVHCollisionTraversalNode() { vertices1 = NULL; vertices2 = NULL; tri_indices1 = NULL; tri_indices2 = NULL; } /// @brief Intersection testing between leaves (two triangles) void leafTesting(int b1, int b2) const { if(this->enable_statistics) this->num_leaf_tests++; const BVNode& node1 = this->model1->getBV(b1); const BVNode& node2 = this->model2->getBV(b2); int primitive_id1 = node1.primitiveId(); int primitive_id2 = node2.primitiveId(); const Triangle& tri_id1 = tri_indices1[primitive_id1]; const Triangle& tri_id2 = tri_indices2[primitive_id2]; const Vec3f& p1 = vertices1[tri_id1[0]]; const Vec3f& p2 = vertices1[tri_id1[1]]; const Vec3f& p3 = vertices1[tri_id1[2]]; const Vec3f& q1 = vertices2[tri_id2[0]]; const Vec3f& q2 = vertices2[tri_id2[1]]; const Vec3f& q3 = vertices2[tri_id2[2]]; if(this->model1->isOccupied() && this->model2->isOccupied()) { bool is_intersect = false; if(!this->request.enable_contact) // only interested in collision or not { if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3)) { is_intersect = true; if(this->result->numContacts() < this->request.num_max_contacts) this->result->addContact(Contact(this->model1, this->model2, primitive_id1, primitive_id2)); } } else // need compute the contact information { FCL_REAL penetration; Vec3f normal; unsigned int n_contacts; Vec3f contacts[2]; if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3, contacts, &n_contacts, &penetration, &normal)) { is_intersect = true; if(this->request.num_max_contacts < n_contacts + this->result->numContacts()) n_contacts = (this->request.num_max_contacts >= this->result->numContacts()) ? (this->request.num_max_contacts - this->result->numContacts()) : 0; for(unsigned int i = 0; i < n_contacts; ++i) { this->result->addContact(Contact(this->model1, this->model2, primitive_id1, primitive_id2, contacts[i], normal, penetration)); } } } if(is_intersect && this->request.enable_cost) { AABB overlap_part; AABB(p1, p2, p3).overlap(AABB(q1, q2, q3), overlap_part); this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources); } } else if((!this->model1->isFree() && !this->model2->isFree()) && this->request.enable_cost) { if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3)) { AABB overlap_part; AABB(p1, p2, p3).overlap(AABB(q1, q2, q3), overlap_part); this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources); } } } /// @brief Whether the traversal process can stop early bool canStop() const { return this->request.isSatisfied(*(this->result)); } Vec3f* vertices1; Vec3f* vertices2; Triangle* tri_indices1; Triangle* tri_indices2; FCL_REAL cost_density; }; /// @brief Traversal node for collision between two meshes if their underlying BVH node is oriented node (OBB, RSS, OBBRSS, kIOS) class MeshCollisionTraversalNodeOBB : public MeshCollisionTraversalNode { public: MeshCollisionTraversalNodeOBB(); bool BVTesting(int b1, int b2) const; void leafTesting(int b1, int b2) const; bool BVTesting(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc) const; void leafTesting(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc) const; Matrix3f R; Vec3f T; }; class MeshCollisionTraversalNodeRSS : public MeshCollisionTraversalNode { public: MeshCollisionTraversalNodeRSS(); bool BVTesting(int b1, int b2) const; void leafTesting(int b1, int b2) const; bool BVTesting(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc) const; void leafTesting(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc) const; Matrix3f R; Vec3f T; }; class MeshCollisionTraversalNodekIOS : public MeshCollisionTraversalNode { public: MeshCollisionTraversalNodekIOS(); bool BVTesting(int b1, int b2) const; void leafTesting(int b1, int b2) const; Matrix3f R; Vec3f T; }; class MeshCollisionTraversalNodeOBBRSS : public MeshCollisionTraversalNode { public: MeshCollisionTraversalNodeOBBRSS(); bool BVTesting(int b1, int b2) const; void leafTesting(int b1, int b2) const; Matrix3f R; Vec3f T; }; /// @brief Traversal node for continuous collision between BVH models struct BVHContinuousCollisionPair { BVHContinuousCollisionPair() {} BVHContinuousCollisionPair(int id1_, int id2_, FCL_REAL time) : id1(id1_), id2(id2_), collision_time(time) {} /// @brief The index of one in-collision primitive int id1; /// @brief The index of the other in-collision primitive int id2; /// @brief Collision time normalized in [0, 1]. The collision time out of [0, 1] means collision-free FCL_REAL collision_time; }; /// @brief Traversal node for continuous collision between meshes template class MeshContinuousCollisionTraversalNode : public BVHCollisionTraversalNode { public: MeshContinuousCollisionTraversalNode() : BVHCollisionTraversalNode() { vertices1 = NULL; vertices2 = NULL; tri_indices1 = NULL; tri_indices2 = NULL; prev_vertices1 = NULL; prev_vertices2 = NULL; num_vf_tests = 0; num_ee_tests = 0; time_of_contact = 1; } /// @brief Intersection testing between leaves (two triangles) void leafTesting(int b1, int b2) const { if(this->enable_statistics) this->num_leaf_tests++; const BVNode& node1 = this->model1->getBV(b1); const BVNode& node2 = this->model2->getBV(b2); FCL_REAL collision_time = 2; Vec3f collision_pos; int primitive_id1 = node1.primitiveId(); int primitive_id2 = node2.primitiveId(); const Triangle& tri_id1 = tri_indices1[primitive_id1]; const Triangle& tri_id2 = tri_indices2[primitive_id2]; Vec3f* S0[3]; Vec3f* S1[3]; Vec3f* T0[3]; Vec3f* T1[3]; for(int i = 0; i < 3; ++i) { S0[i] = prev_vertices1 + tri_id1[i]; S1[i] = vertices1 + tri_id1[i]; T0[i] = prev_vertices2 + tri_id2[i]; T1[i] = vertices2 + tri_id2[i]; } FCL_REAL tmp; Vec3f tmpv; // 6 VF checks for(int i = 0; i < 3; ++i) { if(this->enable_statistics) num_vf_tests++; if(Intersect::intersect_VF(*(S0[0]), *(S0[1]), *(S0[2]), *(T0[i]), *(S1[0]), *(S1[1]), *(S1[2]), *(T1[i]), &tmp, &tmpv)) { if(collision_time > tmp) { collision_time = tmp; collision_pos = tmpv; } } if(this->enable_statistics) num_vf_tests++; if(Intersect::intersect_VF(*(T0[0]), *(T0[1]), *(T0[2]), *(S0[i]), *(T1[0]), *(T1[1]), *(T1[2]), *(S1[i]), &tmp, &tmpv)) { if(collision_time > tmp) { collision_time = tmp; collision_pos = tmpv; } } } // 9 EE checks for(int i = 0; i < 3; ++i) { int S_id1 = i; int S_id2 = i + 1; if(S_id2 == 3) S_id2 = 0; for(int j = 0; j < 3; ++j) { int T_id1 = j; int T_id2 = j + 1; if(T_id2 == 3) T_id2 = 0; num_ee_tests++; if(Intersect::intersect_EE(*(S0[S_id1]), *(S0[S_id2]), *(T0[T_id1]), *(T0[T_id2]), *(S1[S_id1]), *(S1[S_id2]), *(T1[T_id1]), *(T1[T_id2]), &tmp, &tmpv)) { if(collision_time > tmp) { collision_time = tmp; collision_pos = tmpv; } } } } if(!(collision_time > 1)) // collision happens { pairs.push_back(BVHContinuousCollisionPair(primitive_id1, primitive_id2, collision_time)); time_of_contact = std::min(time_of_contact, collision_time); } } /// @brief Whether the traversal process can stop early bool canStop() const { return (pairs.size() > 0) && (this->request.num_max_contacts <= pairs.size()); } Vec3f* vertices1; Vec3f* vertices2; Triangle* tri_indices1; Triangle* tri_indices2; Vec3f* prev_vertices1; Vec3f* prev_vertices2; mutable int num_vf_tests; mutable int num_ee_tests; mutable std::vector pairs; mutable FCL_REAL time_of_contact; }; /// @brief Traversal node for distance computation between BVH models template class BVHDistanceTraversalNode : public DistanceTraversalNodeBase { public: BVHDistanceTraversalNode() : DistanceTraversalNodeBase() { model1 = NULL; model2 = NULL; num_bv_tests = 0; num_leaf_tests = 0; query_time_seconds = 0.0; } /// @brief Whether the BV node in the first BVH tree is leaf bool isFirstNodeLeaf(int b) const { return model1->getBV(b).isLeaf(); } /// @brief Whether the BV node in the second BVH tree is leaf bool isSecondNodeLeaf(int b) const { return model2->getBV(b).isLeaf(); } /// @brief Determine the traversal order, is the first BVTT subtree better bool firstOverSecond(int b1, int b2) const { FCL_REAL sz1 = model1->getBV(b1).bv.size(); FCL_REAL sz2 = model2->getBV(b2).bv.size(); bool l1 = model1->getBV(b1).isLeaf(); bool l2 = model2->getBV(b2).isLeaf(); if(l2 || (!l1 && (sz1 > sz2))) return true; return false; } /// @brief Obtain the left child of BV node in the first BVH int getFirstLeftChild(int b) const { return model1->getBV(b).leftChild(); } /// @brief Obtain the right child of BV node in the first BVH int getFirstRightChild(int b) const { return model1->getBV(b).rightChild(); } /// @brief Obtain the left child of BV node in the second BVH int getSecondLeftChild(int b) const { return model2->getBV(b).leftChild(); } /// @brief Obtain the right child of BV node in the second BVH int getSecondRightChild(int b) const { return model2->getBV(b).rightChild(); } /// @brief BV culling test in one BVTT node FCL_REAL BVTesting(int b1, int b2) const { if(enable_statistics) num_bv_tests++; return model1->getBV(b1).distance(model2->getBV(b2)); } /// @brief The first BVH model const BVHModel* model1; /// @brief The second BVH model const BVHModel* model2; /// @brief statistical information mutable int num_bv_tests; mutable int num_leaf_tests; mutable FCL_REAL query_time_seconds; }; /// @brief Traversal node for distance computation between two meshes template class MeshDistanceTraversalNode : public BVHDistanceTraversalNode { public: MeshDistanceTraversalNode() : BVHDistanceTraversalNode() { vertices1 = NULL; vertices2 = NULL; tri_indices1 = NULL; tri_indices2 = NULL; rel_err = this->request.rel_err; abs_err = this->request.abs_err; } /// @brief Distance testing between leaves (two triangles) void leafTesting(int b1, int b2) const { if(this->enable_statistics) this->num_leaf_tests++; const BVNode& node1 = this->model1->getBV(b1); const BVNode& node2 = this->model2->getBV(b2); int primitive_id1 = node1.primitiveId(); int primitive_id2 = node2.primitiveId(); const Triangle& tri_id1 = tri_indices1[primitive_id1]; const Triangle& tri_id2 = tri_indices2[primitive_id2]; const Vec3f& t11 = vertices1[tri_id1[0]]; const Vec3f& t12 = vertices1[tri_id1[1]]; const Vec3f& t13 = vertices1[tri_id1[2]]; const Vec3f& t21 = vertices2[tri_id2[0]]; const Vec3f& t22 = vertices2[tri_id2[1]]; const Vec3f& t23 = vertices2[tri_id2[2]]; // nearest point pair Vec3f P1, P2; FCL_REAL d = TriangleDistance::triDistance(t11, t12, t13, t21, t22, t23, P1, P2); if(this->request.enable_nearest_points) { this->result->update(d, this->model1, this->model2, primitive_id1, primitive_id2, P1, P2); } else { this->result->update(d, this->model1, this->model2, primitive_id1, primitive_id2); } } /// @brief Whether the traversal process can stop early bool canStop(FCL_REAL c) const { if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance)) return true; return false; } Vec3f* vertices1; Vec3f* vertices2; Triangle* tri_indices1; Triangle* tri_indices2; /// @brief relative and absolute error, default value is 0.01 for both terms FCL_REAL rel_err; FCL_REAL abs_err; }; /// @brief Traversal node for distance computation between two meshes if their underlying BVH node is oriented node (RSS, OBBRSS, kIOS) class MeshDistanceTraversalNodeRSS : public MeshDistanceTraversalNode { public: MeshDistanceTraversalNodeRSS(); void preprocess(); void postprocess(); FCL_REAL BVTesting(int b1, int b2) const; void leafTesting(int b1, int b2) const; Matrix3f R; Vec3f T; }; class MeshDistanceTraversalNodekIOS : public MeshDistanceTraversalNode { public: MeshDistanceTraversalNodekIOS(); void preprocess(); void postprocess(); FCL_REAL BVTesting(int b1, int b2) const; void leafTesting(int b1, int b2) const; Matrix3f R; Vec3f T; }; class MeshDistanceTraversalNodeOBBRSS : public MeshDistanceTraversalNode { public: MeshDistanceTraversalNodeOBBRSS(); void preprocess(); void postprocess(); FCL_REAL BVTesting(int b1, int b2) const; void leafTesting(int b1, int b2) const; Matrix3f R; Vec3f T; }; /// @brief continuous collision node using conservative advancement. when using this default version, must refit the BVH in current configuration (R_t, T_t) into default configuration template class MeshConservativeAdvancementTraversalNode : public MeshDistanceTraversalNode { public: MeshConservativeAdvancementTraversalNode(FCL_REAL w_ = 1) : MeshDistanceTraversalNode() { delta_t = 1; toc = 0; t_err = (FCL_REAL)0.00001; w = w_; motion1 = NULL; motion2 = NULL; } /// @brief BV culling test in one BVTT node FCL_REAL BVTesting(int b1, int b2) const { if(this->enable_statistics) this->num_bv_tests++; Vec3f P1, P2; FCL_REAL d = this->model1->getBV(b1).distance(this->model2->getBV(b2), &P1, &P2); stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d)); return d; } /// @brief Conservative advancement testing between leaves (two triangles) void leafTesting(int b1, int b2) const { if(this->enable_statistics) this->num_leaf_tests++; const BVNode& node1 = this->model1->getBV(b1); const BVNode& node2 = this->model2->getBV(b2); int primitive_id1 = node1.primitiveId(); int primitive_id2 = node2.primitiveId(); const Triangle& tri_id1 = this->tri_indices1[primitive_id1]; const Triangle& tri_id2 = this->tri_indices2[primitive_id2]; const Vec3f& p1 = this->vertices1[tri_id1[0]]; const Vec3f& p2 = this->vertices1[tri_id1[1]]; const Vec3f& p3 = this->vertices1[tri_id1[2]]; const Vec3f& q1 = this->vertices2[tri_id2[0]]; const Vec3f& q2 = this->vertices2[tri_id2[1]]; const Vec3f& q3 = this->vertices2[tri_id2[2]]; // nearest point pair Vec3f P1, P2; FCL_REAL d = TriangleDistance::triDistance(p1, p2, p3, q1, q2, q3, P1, P2); if(d < this->min_distance) { this->min_distance = d; closest_p1 = P1; closest_p2 = P2; last_tri_id1 = primitive_id1; last_tri_id2 = primitive_id2; } Vec3f n = P2 - P1; n.normalize(); // here n is already in global frame as we assume the body is in original configuration (I, 0) for general BVH TriangleMotionBoundVisitor mb_visitor1(p1, p2, p3, n), mb_visitor2(q1, q2, q3, n); FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1); FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2); FCL_REAL bound = bound1 + bound2; FCL_REAL cur_delta_t; if(bound <= d) cur_delta_t = 1; else cur_delta_t = d / bound; if(cur_delta_t < delta_t) delta_t = cur_delta_t; } /// @brief Whether the traversal process can stop early bool canStop(FCL_REAL c) const { if((c >= w * (this->min_distance - this->abs_err)) && (c * (1 + this->rel_err) >= w * this->min_distance)) { const ConservativeAdvancementStackData& data = stack.back(); FCL_REAL d = data.d; Vec3f n; int c1, c2; if(d > c) { const ConservativeAdvancementStackData& data2 = stack[stack.size() - 2]; d = data2.d; n = data2.P2 - data2.P1; n.normalize(); c1 = data2.c1; c2 = data2.c2; stack[stack.size() - 2] = stack[stack.size() - 1]; } else { n = data.P2 - data.P1; n.normalize(); c1 = data.c1; c2 = data.c2; } assert(c == d); TBVMotionBoundVisitor mb_visitor1(this->model1->getBV(c1).bv, n), mb_visitor2(this->model2->getBV(c2).bv, n); FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1); FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2); FCL_REAL bound = bound1 + bound2; FCL_REAL cur_delta_t; if(bound <= c) cur_delta_t = 1; else cur_delta_t = c / bound; if(cur_delta_t < delta_t) delta_t = cur_delta_t; stack.pop_back(); return true; } else { const ConservativeAdvancementStackData& data = stack.back(); FCL_REAL d = data.d; if(d > c) stack[stack.size() - 2] = stack[stack.size() - 1]; stack.pop_back(); return false; } } mutable FCL_REAL min_distance; mutable Vec3f closest_p1, closest_p2; mutable int last_tri_id1, last_tri_id2; /// @brief CA controlling variable: early stop for the early iterations of CA FCL_REAL w; /// @brief The time from beginning point FCL_REAL toc; FCL_REAL t_err; /// @brief The delta_t each step mutable FCL_REAL delta_t; /// @brief Motions for the two objects in query const MotionBase* motion1; const MotionBase* motion2; mutable std::vector stack; }; /// @brief for OBB and RSS, there is local coordinate of BV, so normal need to be transformed namespace details { template const Vec3f& getBVAxis(const BV& bv, int i) { return bv.axis[i]; } template<> inline const Vec3f& getBVAxis(const OBBRSS& bv, int i) { return bv.obb.axis[i]; } template bool meshConservativeAdvancementTraversalNodeCanStop(FCL_REAL c, FCL_REAL min_distance, FCL_REAL abs_err, FCL_REAL rel_err, FCL_REAL w, const BVHModel* model1, const BVHModel* model2, const MotionBase* motion1, const MotionBase* motion2, std::vector& stack, FCL_REAL& delta_t) { if((c >= w * (min_distance - abs_err)) && (c * (1 + rel_err) >= w * min_distance)) { const ConservativeAdvancementStackData& data = stack.back(); FCL_REAL d = data.d; Vec3f n; int c1, c2; if(d > c) { const ConservativeAdvancementStackData& data2 = stack[stack.size() - 2]; d = data2.d; n = data2.P2 - data2.P1; n.normalize(); c1 = data2.c1; c2 = data2.c2; stack[stack.size() - 2] = stack[stack.size() - 1]; } else { n = data.P2 - data.P1; n.normalize(); c1 = data.c1; c2 = data.c2; } assert(c == d); Vec3f n_transformed = getBVAxis(model1->getBV(c1).bv, 0) * n[0] + getBVAxis(model1->getBV(c1).bv, 1) * n[1] + getBVAxis(model1->getBV(c1).bv, 2) * n[2]; TBVMotionBoundVisitor mb_visitor1(model1->getBV(c1).bv, n_transformed), mb_visitor2(model2->getBV(c2).bv, n_transformed); FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1); FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2); FCL_REAL bound = bound1 + bound2; FCL_REAL cur_delta_t; if(bound <= c) cur_delta_t = 1; else cur_delta_t = c / bound; if(cur_delta_t < delta_t) delta_t = cur_delta_t; stack.pop_back(); return true; } else { const ConservativeAdvancementStackData& data = stack.back(); FCL_REAL d = data.d; if(d > c) stack[stack.size() - 2] = stack[stack.size() - 1]; stack.pop_back(); return false; } } } /// for OBB, RSS and OBBRSS, there is local coordinate of BV, so normal need to be transformed template<> inline bool MeshConservativeAdvancementTraversalNode::canStop(FCL_REAL c) const { return details::meshConservativeAdvancementTraversalNodeCanStop(c, this->min_distance, this->abs_err, this->rel_err, w, this->model1, this->model2, motion1, motion2, stack, delta_t); } template<> inline bool MeshConservativeAdvancementTraversalNode::canStop(FCL_REAL c) const { return details::meshConservativeAdvancementTraversalNodeCanStop(c, this->min_distance, this->abs_err, this->rel_err, w, this->model1, this->model2, motion1, motion2, stack, delta_t); } template<> inline bool MeshConservativeAdvancementTraversalNode::canStop(FCL_REAL c) const { return details::meshConservativeAdvancementTraversalNodeCanStop(c, this->min_distance, this->abs_err, this->rel_err, w, this->model1, this->model2, motion1, motion2, stack, delta_t); } class MeshConservativeAdvancementTraversalNodeRSS : public MeshConservativeAdvancementTraversalNode { public: MeshConservativeAdvancementTraversalNodeRSS(FCL_REAL w_ = 1); FCL_REAL BVTesting(int b1, int b2) const; void leafTesting(int b1, int b2) const; bool canStop(FCL_REAL c) const; Matrix3f R; Vec3f T; }; class MeshConservativeAdvancementTraversalNodeOBBRSS : public MeshConservativeAdvancementTraversalNode { public: MeshConservativeAdvancementTraversalNodeOBBRSS(FCL_REAL w_ = 1); FCL_REAL BVTesting(int b1, int b2) const; void leafTesting(int b1, int b2) const; bool canStop(FCL_REAL c) const; Matrix3f R; Vec3f T; }; } #endif fcl-0.5.0/include/fcl/traversal/traversal_node_octree.h000066400000000000000000001221141274356571000232070ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_TRAVERSAL_NODE_OCTREE_H #define FCL_TRAVERSAL_NODE_OCTREE_H #include "fcl/config.h" #if not(FCL_HAVE_OCTOMAP) #error "This header requires fcl to be compiled with octomap support" #endif #include "fcl/collision_data.h" #include "fcl/traversal/traversal_node_base.h" #include "fcl/narrowphase/narrowphase.h" #include "fcl/shape/geometric_shapes_utility.h" #include "fcl/octree.h" #include "fcl/BVH/BVH_model.h" namespace fcl { /// @brief Algorithms for collision related with octree template class OcTreeSolver { private: const NarrowPhaseSolver* solver; mutable const CollisionRequest* crequest; mutable const DistanceRequest* drequest; mutable CollisionResult* cresult; mutable DistanceResult* dresult; public: OcTreeSolver(const NarrowPhaseSolver* solver_) : solver(solver_), crequest(NULL), drequest(NULL), cresult(NULL), dresult(NULL) { } /// @brief collision between two octrees void OcTreeIntersect(const OcTree* tree1, const OcTree* tree2, const Transform3f& tf1, const Transform3f& tf2, const CollisionRequest& request_, CollisionResult& result_) const { crequest = &request_; cresult = &result_; OcTreeIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), tree2, tree2->getRoot(), tree2->getRootBV(), tf1, tf2); } /// @brief distance between two octrees void OcTreeDistance(const OcTree* tree1, const OcTree* tree2, const Transform3f& tf1, const Transform3f& tf2, const DistanceRequest& request_, DistanceResult& result_) const { drequest = &request_; dresult = &result_; OcTreeDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), tree2, tree2->getRoot(), tree2->getRootBV(), tf1, tf2); } /// @brief collision between octree and mesh template void OcTreeMeshIntersect(const OcTree* tree1, const BVHModel* tree2, const Transform3f& tf1, const Transform3f& tf2, const CollisionRequest& request_, CollisionResult& result_) const { crequest = &request_; cresult = &result_; OcTreeMeshIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), tree2, 0, tf1, tf2); } /// @brief distance between octree and mesh template void OcTreeMeshDistance(const OcTree* tree1, const BVHModel* tree2, const Transform3f& tf1, const Transform3f& tf2, const DistanceRequest& request_, DistanceResult& result_) const { drequest = &request_; dresult = &result_; OcTreeMeshDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), tree2, 0, tf1, tf2); } /// @brief collision between mesh and octree template void MeshOcTreeIntersect(const BVHModel* tree1, const OcTree* tree2, const Transform3f& tf1, const Transform3f& tf2, const CollisionRequest& request_, CollisionResult& result_) const { crequest = &request_; cresult = &result_; OcTreeMeshIntersectRecurse(tree2, tree2->getRoot(), tree2->getRootBV(), tree1, 0, tf2, tf1); } /// @brief distance between mesh and octree template void MeshOcTreeDistance(const BVHModel* tree1, const OcTree* tree2, const Transform3f& tf1, const Transform3f& tf2, const DistanceRequest& request_, DistanceResult& result_) const { drequest = &request_; dresult = &result_; OcTreeMeshDistanceRecurse(tree1, 0, tree2, tree2->getRoot(), tree2->getRootBV(), tf1, tf2); } /// @brief collision between octree and shape template void OcTreeShapeIntersect(const OcTree* tree, const S& s, const Transform3f& tf1, const Transform3f& tf2, const CollisionRequest& request_, CollisionResult& result_) const { crequest = &request_; cresult = &result_; AABB bv2; computeBV(s, Transform3f(), bv2); OBB obb2; convertBV(bv2, tf2, obb2); OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), s, obb2, tf1, tf2); } /// @brief collision between shape and octree template void ShapeOcTreeIntersect(const S& s, const OcTree* tree, const Transform3f& tf1, const Transform3f& tf2, const CollisionRequest& request_, CollisionResult& result_) const { crequest = &request_; cresult = &result_; AABB bv1; computeBV(s, Transform3f(), bv1); OBB obb1; convertBV(bv1, tf1, obb1); OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), s, obb1, tf2, tf1); } /// @brief distance between octree and shape template void OcTreeShapeDistance(const OcTree* tree, const S& s, const Transform3f& tf1, const Transform3f& tf2, const DistanceRequest& request_, DistanceResult& result_) const { drequest = &request_; dresult = &result_; AABB aabb2; computeBV(s, tf2, aabb2); OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(), s, aabb2, tf1, tf2); } /// @brief distance between shape and octree template void ShapeOcTreeDistance(const S& s, const OcTree* tree, const Transform3f& tf1, const Transform3f& tf2, const DistanceRequest& request_, DistanceResult& result_) const { drequest = &request_; dresult = &result_; AABB aabb1; computeBV(s, tf1, aabb1); OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(), s, aabb1, tf2, tf1); } private: template bool OcTreeShapeDistanceRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1, const S& s, const AABB& aabb2, const Transform3f& tf1, const Transform3f& tf2) const { if(!tree1->nodeHasChildren(root1)) { if(tree1->isNodeOccupied(root1)) { Box box; Transform3f box_tf; constructBox(bv1, tf1, box, box_tf); FCL_REAL dist; Vec3f closest_p1, closest_p2; solver->shapeDistance(box, box_tf, s, tf2, &dist, &closest_p1, &closest_p2); dresult->update(dist, tree1, &s, root1 - tree1->getRoot(), DistanceResult::NONE, closest_p1, closest_p2); return drequest->isSatisfied(*dresult); } else return false; } if(!tree1->isNodeOccupied(root1)) return false; for(unsigned int i = 0; i < 8; ++i) { if(tree1->nodeChildExists(root1, i)) { const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); AABB child_bv; computeChildBV(bv1, i, child_bv); AABB aabb1; convertBV(child_bv, tf1, aabb1); FCL_REAL d = aabb1.distance(aabb2); if(d < dresult->min_distance) { if(OcTreeShapeDistanceRecurse(tree1, child, child_bv, s, aabb2, tf1, tf2)) return true; } } } return false; } template bool OcTreeShapeIntersectRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1, const S& s, const OBB& obb2, const Transform3f& tf1, const Transform3f& tf2) const { if(!root1) { OBB obb1; convertBV(bv1, tf1, obb1); if(obb1.overlap(obb2)) { Box box; Transform3f box_tf; constructBox(bv1, tf1, box, box_tf); if(solver->shapeIntersect(box, box_tf, s, tf2, NULL)) { AABB overlap_part; AABB aabb1, aabb2; computeBV(box, box_tf, aabb1); computeBV(s, tf2, aabb2); aabb1.overlap(aabb2, overlap_part); cresult->addCostSource(CostSource(overlap_part, tree1->getOccupancyThres() * s.cost_density), crequest->num_max_cost_sources); } } return false; } else if(!tree1->nodeHasChildren(root1)) { if(tree1->isNodeOccupied(root1) && s.isOccupied()) // occupied area { OBB obb1; convertBV(bv1, tf1, obb1); if(obb1.overlap(obb2)) { Box box; Transform3f box_tf; constructBox(bv1, tf1, box, box_tf); bool is_intersect = false; if(!crequest->enable_contact) { if(solver->shapeIntersect(box, box_tf, s, tf2, NULL)) { is_intersect = true; if(cresult->numContacts() < crequest->num_max_contacts) cresult->addContact(Contact(tree1, &s, root1 - tree1->getRoot(), Contact::NONE)); } } else { std::vector contacts; if(solver->shapeIntersect(box, box_tf, s, tf2, &contacts)) { is_intersect = true; if(crequest->num_max_contacts > cresult->numContacts()) { const size_t free_space = crequest->num_max_contacts - cresult->numContacts(); size_t num_adding_contacts; // If the free space is not enough to add all the new contacts, we add contacts in descent order of penetration depth. if (free_space < contacts.size()) { std::partial_sort(contacts.begin(), contacts.begin() + free_space, contacts.end(), std::bind(comparePenDepth, std::placeholders::_2, std::placeholders::_1)); num_adding_contacts = free_space; } else { num_adding_contacts = contacts.size(); } for(size_t i = 0; i < num_adding_contacts; ++i) cresult->addContact(Contact(tree1, &s, root1 - tree1->getRoot(), Contact::NONE, contacts[i].pos, contacts[i].normal, contacts[i].penetration_depth)); } } } if(is_intersect && crequest->enable_cost) { AABB overlap_part; AABB aabb1, aabb2; computeBV(box, box_tf, aabb1); computeBV(s, tf2, aabb2); aabb1.overlap(aabb2, overlap_part); } return crequest->isSatisfied(*cresult); } else return false; } else if(!tree1->isNodeFree(root1) && !s.isFree() && crequest->enable_cost) // uncertain area { OBB obb1; convertBV(bv1, tf1, obb1); if(obb1.overlap(obb2)) { Box box; Transform3f box_tf; constructBox(bv1, tf1, box, box_tf); if(solver->shapeIntersect(box, box_tf, s, tf2, NULL)) { AABB overlap_part; AABB aabb1, aabb2; computeBV(box, box_tf, aabb1); computeBV(s, tf2, aabb2); aabb1.overlap(aabb2, overlap_part); } } return false; } else // free area return false; } /// stop when 1) bounding boxes of two objects not overlap; OR /// 2) at least of one the nodes is free; OR /// 2) (two uncertain nodes or one node occupied and one node uncertain) AND cost not required if(tree1->isNodeFree(root1) || s.isFree()) return false; else if((tree1->isNodeUncertain(root1) || s.isUncertain()) && !crequest->enable_cost) return false; else { OBB obb1; convertBV(bv1, tf1, obb1); if(!obb1.overlap(obb2)) return false; } for(unsigned int i = 0; i < 8; ++i) { if(tree1->nodeChildExists(root1, i)) { const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); AABB child_bv; computeChildBV(bv1, i, child_bv); if(OcTreeShapeIntersectRecurse(tree1, child, child_bv, s, obb2, tf1, tf2)) return true; } else if(!s.isFree() && crequest->enable_cost) { AABB child_bv; computeChildBV(bv1, i, child_bv); if(OcTreeShapeIntersectRecurse(tree1, NULL, child_bv, s, obb2, tf1, tf2)) return true; } } return false; } template bool OcTreeMeshDistanceRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1, const BVHModel* tree2, int root2, const Transform3f& tf1, const Transform3f& tf2) const { if(!tree1->nodeHasChildren(root1) && tree2->getBV(root2).isLeaf()) { if(tree1->isNodeOccupied(root1)) { Box box; Transform3f box_tf; constructBox(bv1, tf1, box, box_tf); int primitive_id = tree2->getBV(root2).primitiveId(); const Triangle& tri_id = tree2->tri_indices[primitive_id]; const Vec3f& p1 = tree2->vertices[tri_id[0]]; const Vec3f& p2 = tree2->vertices[tri_id[1]]; const Vec3f& p3 = tree2->vertices[tri_id[2]]; FCL_REAL dist; Vec3f closest_p1, closest_p2; solver->shapeTriangleDistance(box, box_tf, p1, p2, p3, tf2, &dist, &closest_p1, &closest_p2); dresult->update(dist, tree1, tree2, root1 - tree1->getRoot(), primitive_id); return drequest->isSatisfied(*dresult); } else return false; } if(!tree1->isNodeOccupied(root1)) return false; if(tree2->getBV(root2).isLeaf() || (tree1->nodeHasChildren(root1) && (bv1.size() > tree2->getBV(root2).bv.size()))) { for(unsigned int i = 0; i < 8; ++i) { if(tree1->nodeChildExists(root1, i)) { const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); AABB child_bv; computeChildBV(bv1, i, child_bv); FCL_REAL d; AABB aabb1, aabb2; convertBV(child_bv, tf1, aabb1); convertBV(tree2->getBV(root2).bv, tf2, aabb2); d = aabb1.distance(aabb2); if(d < dresult->min_distance) { if(OcTreeMeshDistanceRecurse(tree1, child, child_bv, tree2, root2, tf1, tf2)) return true; } } } } else { FCL_REAL d; AABB aabb1, aabb2; convertBV(bv1, tf1, aabb1); int child = tree2->getBV(root2).leftChild(); convertBV(tree2->getBV(child).bv, tf2, aabb2); d = aabb1.distance(aabb2); if(d < dresult->min_distance) { if(OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, tf2)) return true; } child = tree2->getBV(root2).rightChild(); convertBV(tree2->getBV(child).bv, tf2, aabb2); d = aabb1.distance(aabb2); if(d < dresult->min_distance) { if(OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, tf2)) return true; } } return false; } template bool OcTreeMeshIntersectRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1, const BVHModel* tree2, int root2, const Transform3f& tf1, const Transform3f& tf2) const { if(!root1) { if(tree2->getBV(root2).isLeaf()) { OBB obb1, obb2; convertBV(bv1, tf1, obb1); convertBV(tree2->getBV(root2).bv, tf2, obb2); if(obb1.overlap(obb2)) { Box box; Transform3f box_tf; constructBox(bv1, tf1, box, box_tf); int primitive_id = tree2->getBV(root2).primitiveId(); const Triangle& tri_id = tree2->tri_indices[primitive_id]; const Vec3f& p1 = tree2->vertices[tri_id[0]]; const Vec3f& p2 = tree2->vertices[tri_id[1]]; const Vec3f& p3 = tree2->vertices[tri_id[2]]; if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2, NULL, NULL, NULL)) { AABB overlap_part; AABB aabb1; computeBV(box, box_tf, aabb1); AABB aabb2(tf2.transform(p1), tf2.transform(p2), tf2.transform(p3)); aabb1.overlap(aabb2, overlap_part); cresult->addCostSource(CostSource(overlap_part, tree1->getOccupancyThres() * tree2->cost_density), crequest->num_max_cost_sources); } } return false; } else { if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).leftChild(), tf1, tf2)) return true; if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).rightChild(), tf1, tf2)) return true; return false; } } else if(!tree1->nodeHasChildren(root1) && tree2->getBV(root2).isLeaf()) { if(tree1->isNodeOccupied(root1) && tree2->isOccupied()) { OBB obb1, obb2; convertBV(bv1, tf1, obb1); convertBV(tree2->getBV(root2).bv, tf2, obb2); if(obb1.overlap(obb2)) { Box box; Transform3f box_tf; constructBox(bv1, tf1, box, box_tf); int primitive_id = tree2->getBV(root2).primitiveId(); const Triangle& tri_id = tree2->tri_indices[primitive_id]; const Vec3f& p1 = tree2->vertices[tri_id[0]]; const Vec3f& p2 = tree2->vertices[tri_id[1]]; const Vec3f& p3 = tree2->vertices[tri_id[2]]; bool is_intersect = false; if(!crequest->enable_contact) { if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2, NULL, NULL, NULL)) { is_intersect = true; if(cresult->numContacts() < crequest->num_max_contacts) cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), primitive_id)); } } else { Vec3f contact; FCL_REAL depth; Vec3f normal; if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2, &contact, &depth, &normal)) { is_intersect = true; if(cresult->numContacts() < crequest->num_max_contacts) cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), primitive_id, contact, normal, depth)); } } if(is_intersect && crequest->enable_cost) { AABB overlap_part; AABB aabb1; computeBV(box, box_tf, aabb1); AABB aabb2(tf2.transform(p1), tf2.transform(p2), tf2.transform(p3)); aabb1.overlap(aabb2, overlap_part); cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * tree2->cost_density), crequest->num_max_cost_sources); } return crequest->isSatisfied(*cresult); } else return false; } else if(!tree1->isNodeFree(root1) && !tree2->isFree() && crequest->enable_cost) // uncertain area { OBB obb1, obb2; convertBV(bv1, tf1, obb1); convertBV(tree2->getBV(root2).bv, tf2, obb2); if(obb1.overlap(obb2)) { Box box; Transform3f box_tf; constructBox(bv1, tf1, box, box_tf); int primitive_id = tree2->getBV(root2).primitiveId(); const Triangle& tri_id = tree2->tri_indices[primitive_id]; const Vec3f& p1 = tree2->vertices[tri_id[0]]; const Vec3f& p2 = tree2->vertices[tri_id[1]]; const Vec3f& p3 = tree2->vertices[tri_id[2]]; if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2, NULL, NULL, NULL)) { AABB overlap_part; AABB aabb1; computeBV(box, box_tf, aabb1); AABB aabb2(tf2.transform(p1), tf2.transform(p2), tf2.transform(p3)); aabb1.overlap(aabb2, overlap_part); cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * tree2->cost_density), crequest->num_max_cost_sources); } } return false; } else // free area return false; } /// stop when 1) bounding boxes of two objects not overlap; OR /// 2) at least one of the nodes is free; OR /// 2) (two uncertain nodes OR one node occupied and one node uncertain) AND cost not required if(tree1->isNodeFree(root1) || tree2->isFree()) return false; else if((tree1->isNodeUncertain(root1) || tree2->isUncertain()) && !crequest->enable_cost) return false; else { OBB obb1, obb2; convertBV(bv1, tf1, obb1); convertBV(tree2->getBV(root2).bv, tf2, obb2); if(!obb1.overlap(obb2)) return false; } if(tree2->getBV(root2).isLeaf() || (tree1->nodeHasChildren(root1) && (bv1.size() > tree2->getBV(root2).bv.size()))) { for(unsigned int i = 0; i < 8; ++i) { if(tree1->nodeChildExists(root1, i)) { const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); AABB child_bv; computeChildBV(bv1, i, child_bv); if(OcTreeMeshIntersectRecurse(tree1, child, child_bv, tree2, root2, tf1, tf2)) return true; } else if(!tree2->isFree() && crequest->enable_cost) { AABB child_bv; computeChildBV(bv1, i, child_bv); if(OcTreeMeshIntersectRecurse(tree1, NULL, child_bv, tree2, root2, tf1, tf2)) return true; } } } else { if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).leftChild(), tf1, tf2)) return true; if(OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2, tree2->getBV(root2).rightChild(), tf1, tf2)) return true; } return false; } bool OcTreeDistanceRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& bv2, const Transform3f& tf1, const Transform3f& tf2) const { if(!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2)) { if(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)) { Box box1, box2; Transform3f box1_tf, box2_tf; constructBox(bv1, tf1, box1, box1_tf); constructBox(bv2, tf2, box2, box2_tf); FCL_REAL dist; Vec3f closest_p1, closest_p2; solver->shapeDistance(box1, box1_tf, box2, box2_tf, &dist, &closest_p1, &closest_p2); dresult->update(dist, tree1, tree2, root1 - tree1->getRoot(), root2 - tree2->getRoot(), closest_p1, closest_p2); return drequest->isSatisfied(*dresult); } else return false; } if(!tree1->isNodeOccupied(root1) || !tree2->isNodeOccupied(root2)) return false; if(!tree2->nodeHasChildren(root2) || (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) { for(unsigned int i = 0; i < 8; ++i) { if(tree1->nodeChildExists(root1, i)) { const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); AABB child_bv; computeChildBV(bv1, i, child_bv); FCL_REAL d; AABB aabb1, aabb2; convertBV(bv1, tf1, aabb1); convertBV(bv2, tf2, aabb2); d = aabb1.distance(aabb2); if(d < dresult->min_distance) { if(OcTreeDistanceRecurse(tree1, child, child_bv, tree2, root2, bv2, tf1, tf2)) return true; } } } } else { for(unsigned int i = 0; i < 8; ++i) { if(tree2->nodeChildExists(root2, i)) { const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(bv2, i, child_bv); FCL_REAL d; AABB aabb1, aabb2; convertBV(bv1, tf1, aabb1); convertBV(bv2, tf2, aabb2); d = aabb1.distance(aabb2); if(d < dresult->min_distance) { if(OcTreeDistanceRecurse(tree1, root1, bv1, tree2, child, child_bv, tf1, tf2)) return true; } } } } return false; } bool OcTreeIntersectRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& bv2, const Transform3f& tf1, const Transform3f& tf2) const { if(!root1 && !root2) { OBB obb1, obb2; convertBV(bv1, tf1, obb1); convertBV(bv2, tf2, obb2); if(obb1.overlap(obb2)) { Box box1, box2; Transform3f box1_tf, box2_tf; constructBox(bv1, tf1, box1, box1_tf); constructBox(bv2, tf2, box2, box2_tf); AABB overlap_part; AABB aabb1, aabb2; computeBV(box1, box1_tf, aabb1); computeBV(box2, box2_tf, aabb2); aabb1.overlap(aabb2, overlap_part); cresult->addCostSource(CostSource(overlap_part, tree1->getOccupancyThres() * tree2->getOccupancyThres()), crequest->num_max_cost_sources); } return false; } else if(!root1 && root2) { if(tree2->nodeHasChildren(root2)) { for(unsigned int i = 0; i < 8; ++i) { if(tree2->nodeChildExists(root2, i)) { const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(bv2, i, child_bv); if(OcTreeIntersectRecurse(tree1, NULL, bv1, tree2, child, child_bv, tf1, tf2)) return true; } else { AABB child_bv; computeChildBV(bv2, i, child_bv); if(OcTreeIntersectRecurse(tree1, NULL, bv1, tree2, NULL, child_bv, tf1, tf2)) return true; } } } else { if(OcTreeIntersectRecurse(tree1, NULL, bv1, tree2, NULL, bv2, tf1, tf2)) return true; } return false; } else if(root1 && !root2) { if(tree1->nodeHasChildren(root1)) { for(unsigned int i = 0; i < 8; ++i) { if(tree1->nodeChildExists(root1, i)) { const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); AABB child_bv; computeChildBV(bv1, i, child_bv); if(OcTreeIntersectRecurse(tree1, child, child_bv, tree2, NULL, bv2, tf1, tf2)) return true; } else { AABB child_bv; computeChildBV(bv1, i, child_bv); if(OcTreeIntersectRecurse(tree1, NULL, child_bv, tree2, NULL, bv2, tf1, tf2)) return true; } } } else { if(OcTreeIntersectRecurse(tree1, NULL, bv1, tree2, NULL, bv2, tf1, tf2)) return true; } return false; } else if(!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2)) { if(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)) // occupied area { bool is_intersect = false; if(!crequest->enable_contact) { OBB obb1, obb2; convertBV(bv1, tf1, obb1); convertBV(bv2, tf2, obb2); if(obb1.overlap(obb2)) { is_intersect = true; if(cresult->numContacts() < crequest->num_max_contacts) cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), root2 - tree2->getRoot())); } } else { Box box1, box2; Transform3f box1_tf, box2_tf; constructBox(bv1, tf1, box1, box1_tf); constructBox(bv2, tf2, box2, box2_tf); std::vector contacts; if(solver->shapeIntersect(box1, box1_tf, box2, box2_tf, &contacts)) { is_intersect = true; if(crequest->num_max_contacts > cresult->numContacts()) { const size_t free_space = crequest->num_max_contacts - cresult->numContacts(); size_t num_adding_contacts; // If the free space is not enough to add all the new contacts, we add contacts in descent order of penetration depth. if (free_space < contacts.size()) { std::partial_sort(contacts.begin(), contacts.begin() + free_space, contacts.end(), std::bind(comparePenDepth, std::placeholders::_2, std::placeholders::_1)); num_adding_contacts = free_space; } else { num_adding_contacts = contacts.size(); } for(size_t i = 0; i < num_adding_contacts; ++i) cresult->addContact(Contact(tree1, tree2, root1 - tree1->getRoot(), root2 - tree2->getRoot(), contacts[i].pos, contacts[i].normal, contacts[i].penetration_depth)); } } } if(is_intersect && crequest->enable_cost) { Box box1, box2; Transform3f box1_tf, box2_tf; constructBox(bv1, tf1, box1, box1_tf); constructBox(bv2, tf2, box2, box2_tf); AABB overlap_part; AABB aabb1, aabb2; computeBV(box1, box1_tf, aabb1); computeBV(box2, box2_tf, aabb2); aabb1.overlap(aabb2, overlap_part); cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * root2->getOccupancy()), crequest->num_max_cost_sources); } return crequest->isSatisfied(*cresult); } else if(!tree1->isNodeFree(root1) && !tree2->isNodeFree(root2) && crequest->enable_cost) // uncertain area (here means both are uncertain or one uncertain and one occupied) { OBB obb1, obb2; convertBV(bv1, tf1, obb1); convertBV(bv2, tf2, obb2); if(obb1.overlap(obb2)) { Box box1, box2; Transform3f box1_tf, box2_tf; constructBox(bv1, tf1, box1, box1_tf); constructBox(bv2, tf2, box2, box2_tf); AABB overlap_part; AABB aabb1, aabb2; computeBV(box1, box1_tf, aabb1); computeBV(box2, box2_tf, aabb2); aabb1.overlap(aabb2, overlap_part); cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * root2->getOccupancy()), crequest->num_max_cost_sources); } return false; } else // free area (at least one node is free) return false; } /// stop when 1) bounding boxes of two objects not overlap; OR /// 2) at least one of the nodes is free; OR /// 2) (two uncertain nodes OR one node occupied and one node uncertain) AND cost not required if(tree1->isNodeFree(root1) || tree2->isNodeFree(root2)) return false; else if((tree1->isNodeUncertain(root1) || tree2->isNodeUncertain(root2)) && !crequest->enable_cost) return false; else { OBB obb1, obb2; convertBV(bv1, tf1, obb1); convertBV(bv2, tf2, obb2); if(!obb1.overlap(obb2)) return false; } if(!tree2->nodeHasChildren(root2) || (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) { for(unsigned int i = 0; i < 8; ++i) { if(tree1->nodeChildExists(root1, i)) { const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); AABB child_bv; computeChildBV(bv1, i, child_bv); if(OcTreeIntersectRecurse(tree1, child, child_bv, tree2, root2, bv2, tf1, tf2)) return true; } else if(!tree2->isNodeFree(root2) && crequest->enable_cost) { AABB child_bv; computeChildBV(bv1, i, child_bv); if(OcTreeIntersectRecurse(tree1, NULL, child_bv, tree2, root2, bv2, tf1, tf2)) return true; } } } else { for(unsigned int i = 0; i < 8; ++i) { if(tree2->nodeChildExists(root2, i)) { const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(bv2, i, child_bv); if(OcTreeIntersectRecurse(tree1, root1, bv1, tree2, child, child_bv, tf1, tf2)) return true; } else if(!tree1->isNodeFree(root1) && crequest->enable_cost) { AABB child_bv; computeChildBV(bv2, i, child_bv); if(OcTreeIntersectRecurse(tree1, root1, bv1, tree2, NULL, child_bv, tf1, tf2)) return true; } } } return false; } }; /// @brief Traversal node for octree collision template class OcTreeCollisionTraversalNode : public CollisionTraversalNodeBase { public: OcTreeCollisionTraversalNode() { model1 = NULL; model2 = NULL; otsolver = NULL; } bool BVTesting(int, int) const { return false; } void leafTesting(int, int) const { otsolver->OcTreeIntersect(model1, model2, tf1, tf2, request, *result); } const OcTree* model1; const OcTree* model2; Transform3f tf1, tf2; const OcTreeSolver* otsolver; }; /// @brief Traversal node for octree distance template class OcTreeDistanceTraversalNode : public DistanceTraversalNodeBase { public: OcTreeDistanceTraversalNode() { model1 = NULL; model2 = NULL; otsolver = NULL; } FCL_REAL BVTesting(int, int) const { return -1; } void leafTesting(int, int) const { otsolver->OcTreeDistance(model1, model2, tf1, tf2, request, *result); } const OcTree* model1; const OcTree* model2; const OcTreeSolver* otsolver; }; /// @brief Traversal node for shape-octree collision template class ShapeOcTreeCollisionTraversalNode : public CollisionTraversalNodeBase { public: ShapeOcTreeCollisionTraversalNode() { model1 = NULL; model2 = NULL; otsolver = NULL; } bool BVTesting(int, int) const { return false; } void leafTesting(int, int) const { otsolver->OcTreeShapeIntersect(model2, *model1, tf2, tf1, request, *result); } const S* model1; const OcTree* model2; Transform3f tf1, tf2; const OcTreeSolver* otsolver; }; /// @brief Traversal node for octree-shape collision template class OcTreeShapeCollisionTraversalNode : public CollisionTraversalNodeBase { public: OcTreeShapeCollisionTraversalNode() { model1 = NULL; model2 = NULL; otsolver = NULL; } bool BVTesting(int, int) const { return false; } void leafTesting(int, int) const { otsolver->OcTreeShapeIntersect(model1, *model2, tf1, tf2, request, *result); } const OcTree* model1; const S* model2; Transform3f tf1, tf2; const OcTreeSolver* otsolver; }; /// @brief Traversal node for shape-octree distance template class ShapeOcTreeDistanceTraversalNode : public DistanceTraversalNodeBase { public: ShapeOcTreeDistanceTraversalNode() { model1 = NULL; model2 = NULL; otsolver = NULL; } FCL_REAL BVTesting(int, int) const { return -1; } void leafTesting(int, int) const { otsolver->OcTreeShapeDistance(model2, *model1, tf2, tf1, request, *result); } const S* model1; const OcTree* model2; const OcTreeSolver* otsolver; }; /// @brief Traversal node for octree-shape distance template class OcTreeShapeDistanceTraversalNode : public DistanceTraversalNodeBase { public: OcTreeShapeDistanceTraversalNode() { model1 = NULL; model2 = NULL; otsolver = NULL; } FCL_REAL BVTesting(int, int) const { return -1; } void leafTesting(int, int) const { otsolver->OcTreeShapeDistance(model1, *model2, tf1, tf2, request, *result); } const OcTree* model1; const S* model2; const OcTreeSolver* otsolver; }; /// @brief Traversal node for mesh-octree collision template class MeshOcTreeCollisionTraversalNode : public CollisionTraversalNodeBase { public: MeshOcTreeCollisionTraversalNode() { model1 = NULL; model2 = NULL; otsolver = NULL; } bool BVTesting(int, int) const { return false; } void leafTesting(int, int) const { otsolver->OcTreeMeshIntersect(model2, model1, tf2, tf1, request, *result); } const BVHModel* model1; const OcTree* model2; Transform3f tf1, tf2; const OcTreeSolver* otsolver; }; /// @brief Traversal node for octree-mesh collision template class OcTreeMeshCollisionTraversalNode : public CollisionTraversalNodeBase { public: OcTreeMeshCollisionTraversalNode() { model1 = NULL; model2 = NULL; otsolver = NULL; } bool BVTesting(int, int) const { return false; } void leafTesting(int, int) const { otsolver->OcTreeMeshIntersect(model1, model2, tf1, tf2, request, *result); } const OcTree* model1; const BVHModel* model2; Transform3f tf1, tf2; const OcTreeSolver* otsolver; }; /// @brief Traversal node for mesh-octree distance template class MeshOcTreeDistanceTraversalNode : public DistanceTraversalNodeBase { public: MeshOcTreeDistanceTraversalNode() { model1 = NULL; model2 = NULL; otsolver = NULL; } FCL_REAL BVTesting(int, int) const { return -1; } void leafTesting(int, int) const { otsolver->OcTreeMeshDistance(model2, model1, tf2, tf1, request, *result); } const BVHModel* model1; const OcTree* model2; const OcTreeSolver* otsolver; }; /// @brief Traversal node for octree-mesh distance template class OcTreeMeshDistanceTraversalNode : public DistanceTraversalNodeBase { public: OcTreeMeshDistanceTraversalNode() { model1 = NULL; model2 = NULL; otsolver = NULL; } FCL_REAL BVTesting(int, int) const { return -1; } void leafTesting(int, int) const { otsolver->OcTreeMeshDistance(model1, model2, tf1, tf2, request, *result); } const OcTree* model1; const BVHModel* model2; const OcTreeSolver* otsolver; }; } #endif fcl-0.5.0/include/fcl/traversal/traversal_node_setup.h000066400000000000000000001254221274356571000230730ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_TRAVERSAL_NODE_SETUP_H #define FCL_TRAVERSAL_NODE_SETUP_H #include "fcl/config.h" #include "fcl/traversal/traversal_node_bvhs.h" #include "fcl/traversal/traversal_node_shapes.h" #include "fcl/traversal/traversal_node_bvh_shape.h" #if FCL_HAVE_OCTOMAP #include "fcl/traversal/traversal_node_octree.h" #endif #include "fcl/BVH/BVH_utility.h" namespace fcl { #if FCL_HAVE_OCTOMAP /// @brief Initialize traversal node for collision between two octrees, given current object transform template bool initialize(OcTreeCollisionTraversalNode& node, const OcTree& model1, const Transform3f& tf1, const OcTree& model2, const Transform3f& tf2, const OcTreeSolver* otsolver, const CollisionRequest& request, CollisionResult& result) { node.request = request; node.result = &result; node.model1 = &model1; node.model2 = &model2; node.otsolver = otsolver; node.tf1 = tf1; node.tf2 = tf2; return true; } /// @brief Initialize traversal node for distance between two octrees, given current object transform template bool initialize(OcTreeDistanceTraversalNode& node, const OcTree& model1, const Transform3f& tf1, const OcTree& model2, const Transform3f& tf2, const OcTreeSolver* otsolver, const DistanceRequest& request, DistanceResult& result) { node.request = request; node.result = &result; node.model1 = &model1; node.model2 = &model2; node.otsolver = otsolver; node.tf1 = tf1; node.tf2 = tf2; return true; } /// @brief Initialize traversal node for collision between one shape and one octree, given current object transform template bool initialize(ShapeOcTreeCollisionTraversalNode& node, const S& model1, const Transform3f& tf1, const OcTree& model2, const Transform3f& tf2, const OcTreeSolver* otsolver, const CollisionRequest& request, CollisionResult& result) { node.request = request; node.result = &result; node.model1 = &model1; node.model2 = &model2; node.otsolver = otsolver; node.tf1 = tf1; node.tf2 = tf2; return true; } /// @brief Initialize traversal node for collision between one octree and one shape, given current object transform template bool initialize(OcTreeShapeCollisionTraversalNode& node, const OcTree& model1, const Transform3f& tf1, const S& model2, const Transform3f& tf2, const OcTreeSolver* otsolver, const CollisionRequest& request, CollisionResult& result) { node.request = request; node.result = &result; node.model1 = &model1; node.model2 = &model2; node.otsolver = otsolver; node.tf1 = tf1; node.tf2 = tf2; return true; } /// @brief Initialize traversal node for distance between one shape and one octree, given current object transform template bool initialize(ShapeOcTreeDistanceTraversalNode& node, const S& model1, const Transform3f& tf1, const OcTree& model2, const Transform3f& tf2, const OcTreeSolver* otsolver, const DistanceRequest& request, DistanceResult& result) { node.request = request; node.result = &result; node.model1 = &model1; node.model2 = &model2; node.otsolver = otsolver; node.tf1 = tf1; node.tf2 = tf2; return true; } /// @brief Initialize traversal node for distance between one octree and one shape, given current object transform template bool initialize(OcTreeShapeDistanceTraversalNode& node, const OcTree& model1, const Transform3f& tf1, const S& model2, const Transform3f& tf2, const OcTreeSolver* otsolver, const DistanceRequest& request, DistanceResult& result) { node.request = request; node.result = &result; node.model1 = &model1; node.model2 = &model2; node.otsolver = otsolver; node.tf1 = tf1; node.tf2 = tf2; return true; } /// @brief Initialize traversal node for collision between one mesh and one octree, given current object transform template bool initialize(MeshOcTreeCollisionTraversalNode& node, const BVHModel& model1, const Transform3f& tf1, const OcTree& model2, const Transform3f& tf2, const OcTreeSolver* otsolver, const CollisionRequest& request, CollisionResult& result) { node.request = request; node.result = &result; node.model1 = &model1; node.model2 = &model2; node.otsolver = otsolver; node.tf1 = tf1; node.tf2 = tf2; return true; } /// @brief Initialize traversal node for collision between one octree and one mesh, given current object transform template bool initialize(OcTreeMeshCollisionTraversalNode& node, const OcTree& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, const OcTreeSolver* otsolver, const CollisionRequest& request, CollisionResult& result) { node.request = request; node.result = &result; node.model1 = &model1; node.model2 = &model2; node.otsolver = otsolver; node.tf1 = tf1; node.tf2 = tf2; return true; } /// @brief Initialize traversal node for distance between one mesh and one octree, given current object transform template bool initialize(MeshOcTreeDistanceTraversalNode& node, const BVHModel& model1, const Transform3f& tf1, const OcTree& model2, const Transform3f& tf2, const OcTreeSolver* otsolver, const DistanceRequest& request, DistanceResult& result) { node.request = request; node.result = &result; node.model1 = &model1; node.model2 = &model2; node.otsolver = otsolver; node.tf1 = tf1; node.tf2 = tf2; return true; } /// @brief Initialize traversal node for collision between one octree and one mesh, given current object transform template bool initialize(OcTreeMeshDistanceTraversalNode& node, const OcTree& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, const OcTreeSolver* otsolver, const DistanceRequest& request, DistanceResult& result) { node.request = request; node.result = &result; node.model1 = &model1; node.model2 = &model2; node.otsolver = otsolver; node.tf1 = tf1; node.tf2 = tf2; return true; } #endif /// @brief Initialize traversal node for collision between two geometric shapes, given current object transform template bool initialize(ShapeCollisionTraversalNode& node, const S1& shape1, const Transform3f& tf1, const S2& shape2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { node.model1 = &shape1; node.tf1 = tf1; node.model2 = &shape2; node.tf2 = tf2; node.nsolver = nsolver; node.request = request; node.result = &result; node.cost_density = shape1.cost_density * shape2.cost_density; return true; } /// @brief Initialize traversal node for collision between one mesh and one shape, given current object transform template bool initialize(MeshShapeCollisionTraversalNode& node, BVHModel& model1, Transform3f& tf1, const S& model2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result, bool use_refit = false, bool refit_bottomup = false) { if(model1.getModelType() != BVH_MODEL_TRIANGLES) return false; if(!tf1.isIdentity()) { std::vector vertices_transformed(model1.num_vertices); for(int i = 0; i < model1.num_vertices; ++i) { Vec3f& p = model1.vertices[i]; Vec3f new_v = tf1.transform(p); vertices_transformed[i] = new_v; } model1.beginReplaceModel(); model1.replaceSubModel(vertices_transformed); model1.endReplaceModel(use_refit, refit_bottomup); tf1.setIdentity(); } node.model1 = &model1; node.tf1 = tf1; node.model2 = &model2; node.tf2 = tf2; node.nsolver = nsolver; computeBV(model2, tf2, node.model2_bv); node.vertices = model1.vertices; node.tri_indices = model1.tri_indices; node.request = request; node.result = &result; node.cost_density = model1.cost_density * model2.cost_density; return true; } /// @brief Initialize traversal node for collision between one mesh and one shape, given current object transform template bool initialize(ShapeMeshCollisionTraversalNode& node, const S& model1, const Transform3f& tf1, BVHModel& model2, Transform3f& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result, bool use_refit = false, bool refit_bottomup = false) { if(model2.getModelType() != BVH_MODEL_TRIANGLES) return false; if(!tf2.isIdentity()) { std::vector vertices_transformed(model2.num_vertices); for(int i = 0; i < model2.num_vertices; ++i) { Vec3f& p = model2.vertices[i]; Vec3f new_v = tf2.transform(p); vertices_transformed[i] = new_v; } model2.beginReplaceModel(); model2.replaceSubModel(vertices_transformed); model2.endReplaceModel(use_refit, refit_bottomup); tf2.setIdentity(); } node.model1 = &model1; node.tf1 = tf1; node.model2 = &model2; node.tf2 = tf2; node.nsolver = nsolver; computeBV(model1, tf1, node.model1_bv); node.vertices = model2.vertices; node.tri_indices = model2.tri_indices; node.request = request; node.result = &result; node.cost_density = model1.cost_density * model2.cost_density; return true; } /// @cond IGNORE namespace details { template class OrientedNode> static inline bool setupMeshShapeCollisionOrientedNode(OrientedNode& node, const BVHModel& model1, const Transform3f& tf1, const S& model2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { if(model1.getModelType() != BVH_MODEL_TRIANGLES) return false; node.model1 = &model1; node.tf1 = tf1; node.model2 = &model2; node.tf2 = tf2; node.nsolver = nsolver; computeBV(model2, tf2, node.model2_bv); node.vertices = model1.vertices; node.tri_indices = model1.tri_indices; node.request = request; node.result = &result; node.cost_density = model1.cost_density * model2.cost_density; return true; } } /// @endcond /// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBB type template bool initialize(MeshShapeCollisionTraversalNodeOBB& node, const BVHModel& model1, const Transform3f& tf1, const S& model2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); } /// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for RSS type template bool initialize(MeshShapeCollisionTraversalNodeRSS& node, const BVHModel& model1, const Transform3f& tf1, const S& model2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); } /// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for kIOS type template bool initialize(MeshShapeCollisionTraversalNodekIOS& node, const BVHModel& model1, const Transform3f& tf1, const S& model2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); } /// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBBRSS type template bool initialize(MeshShapeCollisionTraversalNodeOBBRSS& node, const BVHModel& model1, const Transform3f& tf1, const S& model2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); } /// @cond IGNORE namespace details { template class OrientedNode> static inline bool setupShapeMeshCollisionOrientedNode(OrientedNode& node, const S& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { if(model2.getModelType() != BVH_MODEL_TRIANGLES) return false; node.model1 = &model1; node.tf1 = tf1; node.model2 = &model2; node.tf2 = tf2; node.nsolver = nsolver; computeBV(model1, tf1, node.model1_bv); node.vertices = model2.vertices; node.tri_indices = model2.tri_indices; node.request = request; node.result = &result; node.cost_density = model1.cost_density * model2.cost_density; return true; } } /// @endcond /// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBB type template bool initialize(ShapeMeshCollisionTraversalNodeOBB& node, const S& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { return details::setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); } /// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for RSS type template bool initialize(ShapeMeshCollisionTraversalNodeRSS& node, const S& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { return details::setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); } /// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for kIOS type template bool initialize(ShapeMeshCollisionTraversalNodekIOS& node, const S& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { return details::setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); } /// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBBRSS type template bool initialize(ShapeMeshCollisionTraversalNodeOBBRSS& node, const S& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { return details::setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); } /// @brief Initialize traversal node for collision between two meshes, given the current transforms template bool initialize(MeshCollisionTraversalNode& node, BVHModel& model1, Transform3f& tf1, BVHModel& model2, Transform3f& tf2, const CollisionRequest& request, CollisionResult& result, bool use_refit = false, bool refit_bottomup = false) { if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES) return false; if(!tf1.isIdentity()) { std::vector vertices_transformed1(model1.num_vertices); for(int i = 0; i < model1.num_vertices; ++i) { Vec3f& p = model1.vertices[i]; Vec3f new_v = tf1.transform(p); vertices_transformed1[i] = new_v; } model1.beginReplaceModel(); model1.replaceSubModel(vertices_transformed1); model1.endReplaceModel(use_refit, refit_bottomup); tf1.setIdentity(); } if(!tf2.isIdentity()) { std::vector vertices_transformed2(model2.num_vertices); for(int i = 0; i < model2.num_vertices; ++i) { Vec3f& p = model2.vertices[i]; Vec3f new_v = tf2.transform(p); vertices_transformed2[i] = new_v; } model2.beginReplaceModel(); model2.replaceSubModel(vertices_transformed2); model2.endReplaceModel(use_refit, refit_bottomup); tf2.setIdentity(); } node.model1 = &model1; node.tf1 = tf1; node.model2 = &model2; node.tf2 = tf2; node.vertices1 = model1.vertices; node.vertices2 = model2.vertices; node.tri_indices1 = model1.tri_indices; node.tri_indices2 = model2.tri_indices; node.request = request; node.result = &result; node.cost_density = model1.cost_density * model2.cost_density; return true; } /// @brief Initialize traversal node for collision between two meshes, specialized for OBB type bool initialize(MeshCollisionTraversalNodeOBB& node, const BVHModel& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result); /// @brief Initialize traversal node for collision between two meshes, specialized for RSS type bool initialize(MeshCollisionTraversalNodeRSS& node, const BVHModel& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result); /// @brief Initialize traversal node for collision between two meshes, specialized for OBBRSS type bool initialize(MeshCollisionTraversalNodeOBBRSS& node, const BVHModel& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result); /// @brief Initialize traversal node for collision between two meshes, specialized for kIOS type bool initialize(MeshCollisionTraversalNodekIOS& node, const BVHModel& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result); /// @brief Initialize traversal node for distance between two geometric shapes template bool initialize(ShapeDistanceTraversalNode& node, const S1& shape1, const Transform3f& tf1, const S2& shape2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { node.request = request; node.result = &result; node.model1 = &shape1; node.tf1 = tf1; node.model2 = &shape2; node.tf2 = tf2; node.nsolver = nsolver; return true; } /// @brief Initialize traversal node for distance computation between two meshes, given the current transforms template bool initialize(MeshDistanceTraversalNode& node, BVHModel& model1, Transform3f& tf1, BVHModel& model2, Transform3f& tf2, const DistanceRequest& request, DistanceResult& result, bool use_refit = false, bool refit_bottomup = false) { if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES) return false; if(!tf1.isIdentity()) { std::vector vertices_transformed1(model1.num_vertices); for(int i = 0; i < model1.num_vertices; ++i) { Vec3f& p = model1.vertices[i]; Vec3f new_v = tf1.transform(p); vertices_transformed1[i] = new_v; } model1.beginReplaceModel(); model1.replaceSubModel(vertices_transformed1); model1.endReplaceModel(use_refit, refit_bottomup); tf1.setIdentity(); } if(!tf2.isIdentity()) { std::vector vertices_transformed2(model2.num_vertices); for(int i = 0; i < model2.num_vertices; ++i) { Vec3f& p = model2.vertices[i]; Vec3f new_v = tf2.transform(p); vertices_transformed2[i] = new_v; } model2.beginReplaceModel(); model2.replaceSubModel(vertices_transformed2); model2.endReplaceModel(use_refit, refit_bottomup); tf2.setIdentity(); } node.request = request; node.result = &result; node.model1 = &model1; node.tf1 = tf1; node.model2 = &model2; node.tf2 = tf2; node.vertices1 = model1.vertices; node.vertices2 = model2.vertices; node.tri_indices1 = model1.tri_indices; node.tri_indices2 = model2.tri_indices; return true; } /// @brief Initialize traversal node for distance computation between two meshes, specialized for RSS type bool initialize(MeshDistanceTraversalNodeRSS& node, const BVHModel& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, const DistanceRequest& request, DistanceResult& result); /// @brief Initialize traversal node for distance computation between two meshes, specialized for kIOS type bool initialize(MeshDistanceTraversalNodekIOS& node, const BVHModel& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, const DistanceRequest& request, DistanceResult& result); /// @brief Initialize traversal node for distance computation between two meshes, specialized for OBBRSS type bool initialize(MeshDistanceTraversalNodeOBBRSS& node, const BVHModel& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, const DistanceRequest& request, DistanceResult& result); /// @brief Initialize traversal node for distance computation between one mesh and one shape, given the current transforms template bool initialize(MeshShapeDistanceTraversalNode& node, BVHModel& model1, Transform3f& tf1, const S& model2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const DistanceRequest& request, DistanceResult& result, bool use_refit = false, bool refit_bottomup = false) { if(model1.getModelType() != BVH_MODEL_TRIANGLES) return false; if(!tf1.isIdentity()) { std::vector vertices_transformed1(model1.num_vertices); for(int i = 0; i < model1.num_vertices; ++i) { Vec3f& p = model1.vertices[i]; Vec3f new_v = tf1.transform(p); vertices_transformed1[i] = new_v; } model1.beginReplaceModel(); model1.replaceSubModel(vertices_transformed1); model1.endReplaceModel(use_refit, refit_bottomup); tf1.setIdentity(); } node.request = request; node.result = &result; node.model1 = &model1; node.tf1 = tf1; node.model2 = &model2; node.tf2 = tf2; node.nsolver = nsolver; node.vertices = model1.vertices; node.tri_indices = model1.tri_indices; computeBV(model2, tf2, node.model2_bv); return true; } /// @brief Initialize traversal node for distance computation between one shape and one mesh, given the current transforms template bool initialize(ShapeMeshDistanceTraversalNode& node, const S& model1, const Transform3f& tf1, BVHModel& model2, Transform3f& tf2, const NarrowPhaseSolver* nsolver, const DistanceRequest& request, DistanceResult& result, bool use_refit = false, bool refit_bottomup = false) { if(model2.getModelType() != BVH_MODEL_TRIANGLES) return false; if(!tf2.isIdentity()) { std::vector vertices_transformed(model2.num_vertices); for(int i = 0; i < model2.num_vertices; ++i) { Vec3f& p = model2.vertices[i]; Vec3f new_v = tf2.transform(p); vertices_transformed[i] = new_v; } model2.beginReplaceModel(); model2.replaceSubModel(vertices_transformed); model2.endReplaceModel(use_refit, refit_bottomup); tf2.setIdentity(); } node.request = request; node.result = &result; node.model1 = &model1; node.tf1 = tf1; node.model2 = &model2; node.tf2 = tf2; node.nsolver = nsolver; node.vertices = model2.vertices; node.tri_indices = model2.tri_indices; computeBV(model1, tf1, node.model1_bv); return true; } /// @cond IGNORE namespace details { template class OrientedNode> static inline bool setupMeshShapeDistanceOrientedNode(OrientedNode& node, const BVHModel& model1, const Transform3f& tf1, const S& model2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { if(model1.getModelType() != BVH_MODEL_TRIANGLES) return false; node.request = request; node.result = &result; node.model1 = &model1; node.tf1 = tf1; node.model2 = &model2; node.tf2 = tf2; node.nsolver = nsolver; computeBV(model2, tf2, node.model2_bv); node.vertices = model1.vertices; node.tri_indices = model1.tri_indices; return true; } } /// @endcond /// @brief Initialize traversal node for distance computation between one mesh and one shape, specialized for RSS type template bool initialize(MeshShapeDistanceTraversalNodeRSS& node, const BVHModel& model1, const Transform3f& tf1, const S& model2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); } /// @brief Initialize traversal node for distance computation between one mesh and one shape, specialized for kIOS type template bool initialize(MeshShapeDistanceTraversalNodekIOS& node, const BVHModel& model1, const Transform3f& tf1, const S& model2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); } /// @brief Initialize traversal node for distance computation between one mesh and one shape, specialized for OBBRSS type template bool initialize(MeshShapeDistanceTraversalNodeOBBRSS& node, const BVHModel& model1, const Transform3f& tf1, const S& model2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); } namespace details { template class OrientedNode> static inline bool setupShapeMeshDistanceOrientedNode(OrientedNode& node, const S& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { if(model2.getModelType() != BVH_MODEL_TRIANGLES) return false; node.request = request; node.result = &result; node.model1 = &model1; node.tf1 = tf1; node.model2 = &model2; node.tf2 = tf2; node.nsolver = nsolver; computeBV(model1, tf1, node.model1_bv); node.vertices = model2.vertices; node.tri_indices = model2.tri_indices; node.R = tf2.getRotation(); node.T = tf2.getTranslation(); return true; } } /// @brief Initialize traversal node for distance computation between one shape and one mesh, specialized for RSS type template bool initialize(ShapeMeshDistanceTraversalNodeRSS& node, const S& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); } /// @brief Initialize traversal node for distance computation between one shape and one mesh, specialized for kIOS type template bool initialize(ShapeMeshDistanceTraversalNodekIOS& node, const S& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); } /// @brief Initialize traversal node for distance computation between one shape and one mesh, specialized for OBBRSS type template bool initialize(ShapeMeshDistanceTraversalNodeOBBRSS& node, const S& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result); } /// @brief Initialize traversal node for continuous collision detection between two meshes template bool initialize(MeshContinuousCollisionTraversalNode& node, const BVHModel& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, const CollisionRequest& request) { node.model1 = &model1; node.tf1 = tf1; node.model2 = &model2; node.tf2 = tf2; node.vertices1 = model1.vertices; node.vertices2 = model2.vertices; node.tri_indices1 = model1.tri_indices; node.tri_indices2 = model2.tri_indices; node.prev_vertices1 = model1.prev_vertices; node.prev_vertices2 = model2.prev_vertices; node.request = request; return true; } /// @brief Initialize traversal node for conservative advancement computation between two meshes, given the current transforms template bool initialize(MeshConservativeAdvancementTraversalNode& node, BVHModel& model1, const Transform3f& tf1, BVHModel& model2, const Transform3f& tf2, FCL_REAL w = 1, bool use_refit = false, bool refit_bottomup = false) { std::vector vertices_transformed1(model1.num_vertices); for(int i = 0; i < model1.num_vertices; ++i) { Vec3f& p = model1.vertices[i]; Vec3f new_v = tf1.transform(p); vertices_transformed1[i] = new_v; } std::vector vertices_transformed2(model2.num_vertices); for(int i = 0; i < model2.num_vertices; ++i) { Vec3f& p = model2.vertices[i]; Vec3f new_v = tf2.transform(p); vertices_transformed2[i] = new_v; } model1.beginReplaceModel(); model1.replaceSubModel(vertices_transformed1); model1.endReplaceModel(use_refit, refit_bottomup); model2.beginReplaceModel(); model2.replaceSubModel(vertices_transformed2); model2.endReplaceModel(use_refit, refit_bottomup); node.model1 = &model1; node.model2 = &model2; node.vertices1 = model1.vertices; node.vertices2 = model2.vertices; node.tri_indices1 = model1.tri_indices; node.tri_indices2 = model2.tri_indices; node.w = w; return true; } /// @brief Initialize traversal node for conservative advancement computation between two meshes, given the current transforms, specialized for RSS bool initialize(MeshConservativeAdvancementTraversalNodeRSS& node, const BVHModel& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, FCL_REAL w = 1); bool initialize(MeshConservativeAdvancementTraversalNodeOBBRSS& node, const BVHModel& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, FCL_REAL w = 1); template bool initialize(ShapeConservativeAdvancementTraversalNode& node, const S1& shape1, const Transform3f& tf1, const S2& shape2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver) { node.model1 = &shape1; node.tf1 = tf1; node.model2 = &shape2; node.tf2 = tf2; node.nsolver = nsolver; computeBV(shape1, Transform3f(), node.model1_bv); computeBV(shape2, Transform3f(), node.model2_bv); return true; } template bool initialize(MeshShapeConservativeAdvancementTraversalNode& node, BVHModel& model1, const Transform3f& tf1, const S& model2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, FCL_REAL w = 1, bool use_refit = false, bool refit_bottomup = false) { std::vector vertices_transformed(model1.num_vertices); for(int i = 0; i < model1.num_vertices; ++i) { Vec3f& p = model1.vertices[i]; Vec3f new_v = tf1.transform(p); vertices_transformed[i] = new_v; } model1.beginReplaceModel(); model1.replaceSubModel(vertices_transformed); model1.endReplaceModel(use_refit, refit_bottomup); node.model1 = &model1; node.model2 = &model2; node.vertices = model1.vertices; node.tri_indices = model1.tri_indices; node.tf1 = tf1; node.tf2 = tf2; node.nsolver = nsolver; node.w = w; computeBV(model2, Transform3f(), node.model2_bv); return true; } template bool initialize(MeshShapeConservativeAdvancementTraversalNodeRSS& node, const BVHModel& model1, const Transform3f& tf1, const S& model2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, FCL_REAL w = 1) { node.model1 = &model1; node.tf1 = tf1; node.model2 = &model2; node.tf2 = tf2; node.nsolver = nsolver; node.w = w; computeBV(model2, Transform3f(), node.model2_bv); return true; } template bool initialize(MeshShapeConservativeAdvancementTraversalNodeOBBRSS& node, const BVHModel& model1, const Transform3f& tf1, const S& model2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, FCL_REAL w = 1) { node.model1 = &model1; node.tf1 = tf1; node.model2 = &model2; node.tf2 = tf2; node.nsolver = nsolver; node.w = w; computeBV(model2, Transform3f(), node.model2_bv); return true; } template bool initialize(ShapeMeshConservativeAdvancementTraversalNode& node, const S& model1, const Transform3f& tf1, BVHModel& model2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, FCL_REAL w = 1, bool use_refit = false, bool refit_bottomup = false) { std::vector vertices_transformed(model2.num_vertices); for(int i = 0; i < model2.num_vertices; ++i) { Vec3f& p = model2.vertices[i]; Vec3f new_v = tf2.transform(p); vertices_transformed[i] = new_v; } model2.beginReplaceModel(); model2.replaceSubModel(vertices_transformed); model2.endReplaceModel(use_refit, refit_bottomup); node.model1 = &model1; node.model2 = &model2; node.vertices = model2.vertices; node.tri_indices = model2.tri_indices; node.tf1 = tf1; node.tf2 = tf2; node.nsolver = nsolver; node.w = w; computeBV(model1, Transform3f(), node.model1_bv); return true; } template bool initialize(ShapeMeshConservativeAdvancementTraversalNodeRSS& node, const S& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, FCL_REAL w = 1) { node.model1 = &model1; node.tf1 = tf1; node.model2 = &model2; node.tf2 = tf2; node.nsolver = nsolver; node.w = w; computeBV(model1, Transform3f(), node.model1_bv); return true; } template bool initialize(ShapeMeshConservativeAdvancementTraversalNodeOBBRSS& node, const S& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, FCL_REAL w = 1) { node.model1 = &model1; node.tf1 = tf1; node.model2 = &model2; node.tf2 = tf2; node.nsolver = nsolver; node.w = w; computeBV(model1, Transform3f(), node.model1_bv); return true; } } #endif fcl-0.5.0/include/fcl/traversal/traversal_node_shapes.h000066400000000000000000000167631274356571000232250ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_TRAVERSAL_NODE_SHAPES_H #define FCL_TRAVERSAL_NODE_SHAPES_H #include #include "fcl/collision_data.h" #include "fcl/traversal/traversal_node_base.h" #include "fcl/narrowphase/narrowphase.h" #include "fcl/shape/geometric_shapes_utility.h" #include "fcl/BV/BV.h" #include "fcl/shape/geometric_shapes_utility.h" #include "fcl/ccd/motion.h" namespace fcl { /// @brief Traversal node for collision between two shapes template class ShapeCollisionTraversalNode : public CollisionTraversalNodeBase { public: ShapeCollisionTraversalNode() : CollisionTraversalNodeBase() { model1 = NULL; model2 = NULL; nsolver = NULL; } /// @brief BV culling test in one BVTT node bool BVTesting(int, int) const { return false; } /// @brief Intersection testing between leaves (two shapes) void leafTesting(int, int) const { if(model1->isOccupied() && model2->isOccupied()) { bool is_collision = false; if(request.enable_contact) { std::vector contacts; if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, &contacts)) { is_collision = true; if(request.num_max_contacts > result->numContacts()) { const size_t free_space = request.num_max_contacts - result->numContacts(); size_t num_adding_contacts; // If the free space is not enough to add all the new contacts, we add contacts in descent order of penetration depth. if (free_space < contacts.size()) { std::partial_sort(contacts.begin(), contacts.begin() + free_space, contacts.end(), std::bind(comparePenDepth, std::placeholders::_2, std::placeholders::_1)); num_adding_contacts = free_space; } else { num_adding_contacts = contacts.size(); } for(size_t i = 0; i < num_adding_contacts; ++i) result->addContact(Contact(model1, model2, Contact::NONE, Contact::NONE, contacts[i].pos, contacts[i].normal, contacts[i].penetration_depth)); } } } else { if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, NULL)) { is_collision = true; if(request.num_max_contacts > result->numContacts()) result->addContact(Contact(model1, model2, Contact::NONE, Contact::NONE)); } } if(is_collision && request.enable_cost) { AABB aabb1, aabb2; computeBV(*model1, tf1, aabb1); computeBV(*model2, tf2, aabb2); AABB overlap_part; aabb1.overlap(aabb2, overlap_part); result->addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources); } } else if((!model1->isFree() && !model2->isFree()) && request.enable_cost) { if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, NULL)) { AABB aabb1, aabb2; computeBV(*model1, tf1, aabb1); computeBV(*model2, tf2, aabb2); AABB overlap_part; aabb1.overlap(aabb2, overlap_part); result->addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources); } } } const S1* model1; const S2* model2; FCL_REAL cost_density; const NarrowPhaseSolver* nsolver; }; /// @brief Traversal node for distance between two shapes template class ShapeDistanceTraversalNode : public DistanceTraversalNodeBase { public: ShapeDistanceTraversalNode() : DistanceTraversalNodeBase() { model1 = NULL; model2 = NULL; nsolver = NULL; } /// @brief BV culling test in one BVTT node FCL_REAL BVTesting(int, int) const { return -1; // should not be used } /// @brief Distance testing between leaves (two shapes) void leafTesting(int, int) const { FCL_REAL distance; Vec3f closest_p1, closest_p2; nsolver->shapeDistance(*model1, tf1, *model2, tf2, &distance, &closest_p1, &closest_p2); result->update(distance, model1, model2, DistanceResult::NONE, DistanceResult::NONE, closest_p1, closest_p2); } const S1* model1; const S2* model2; const NarrowPhaseSolver* nsolver; }; template class ShapeConservativeAdvancementTraversalNode : public ShapeDistanceTraversalNode { public: ShapeConservativeAdvancementTraversalNode() : ShapeDistanceTraversalNode() { delta_t = 1; toc = 0; t_err = (FCL_REAL)0.0001; motion1 = NULL; motion2 = NULL; } void leafTesting(int, int) const { FCL_REAL distance; Vec3f closest_p1, closest_p2; this->nsolver->shapeDistance(*(this->model1), this->tf1, *(this->model2), this->tf2, &distance, &closest_p1, &closest_p2); Vec3f n = this->tf2.transform(closest_p2) - this->tf1.transform(closest_p1); n.normalize(); TBVMotionBoundVisitor mb_visitor1(model1_bv, n); TBVMotionBoundVisitor mb_visitor2(model2_bv, -n); FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1); FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2); FCL_REAL bound = bound1 + bound2; FCL_REAL cur_delta_t; if(bound <= distance) cur_delta_t = 1; else cur_delta_t = distance / bound; if(cur_delta_t < delta_t) delta_t = cur_delta_t; } mutable FCL_REAL min_distance; /// @brief The time from beginning point FCL_REAL toc; FCL_REAL t_err; /// @brief The delta_t each step mutable FCL_REAL delta_t; /// @brief Motions for the two objects in query const MotionBase* motion1; const MotionBase* motion2; RSS model1_bv, model2_bv; // local bv for the two shapes }; } #endif fcl-0.5.0/include/fcl/traversal/traversal_recurse.h000066400000000000000000000063001274356571000223670ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef FCL_TRAVERSAL_RECURSE_H #define FCL_TRAVERSAL_RECURSE_H #include "fcl/traversal/traversal_node_base.h" #include "fcl/traversal/traversal_node_bvhs.h" #include "fcl/BVH/BVH_front.h" #include namespace fcl { /// @brief Recurse function for collision void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list); /// @brief Recurse function for collision, specialized for OBB type void collisionRecurse(MeshCollisionTraversalNodeOBB* node, int b1, int b2, const Matrix3f& R, const Vec3f& T, BVHFrontList* front_list); /// @brief Recurse function for collision, specialized for RSS type void collisionRecurse(MeshCollisionTraversalNodeRSS* node, int b1, int b2, const Matrix3f& R, const Vec3f& T, BVHFrontList* front_list); /// @brief Recurse function for self collision. Make sure node is set correctly so that the first and second tree are the same void selfCollisionRecurse(CollisionTraversalNodeBase* node, int b, BVHFrontList* front_list); /// @brief Recurse function for distance void distanceRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list); /// @brief Recurse function for distance, using queue acceleration void distanceQueueRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list, int qsize); /// @brief Recurse function for front list propagation void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node, BVHFrontList* front_list); } #endif fcl-0.5.0/src/000077500000000000000000000000001274356571000130615ustar00rootroot00000000000000fcl-0.5.0/src/BV/000077500000000000000000000000001274356571000133705ustar00rootroot00000000000000fcl-0.5.0/src/BV/AABB.cpp000066400000000000000000000066561274356571000145760ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/BV/AABB.h" #include #include namespace fcl { AABB::AABB() : min_(std::numeric_limits::max()), max_(-std::numeric_limits::max()) { } FCL_REAL AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const { FCL_REAL result = 0; for(std::size_t i = 0; i < 3; ++i) { const FCL_REAL& amin = min_[i]; const FCL_REAL& amax = max_[i]; const FCL_REAL& bmin = other.min_[i]; const FCL_REAL& bmax = other.max_[i]; if(amin > bmax) { FCL_REAL delta = bmax - amin; result += delta * delta; if(P && Q) { (*P)[i] = amin; (*Q)[i] = bmax; } } else if(bmin > amax) { FCL_REAL delta = amax - bmin; result += delta * delta; if(P && Q) { (*P)[i] = amax; (*Q)[i] = bmin; } } else { if(P && Q) { if(bmin >= amin) { FCL_REAL t = 0.5 * (amax + bmin); (*P)[i] = t; (*Q)[i] = t; } else { FCL_REAL t = 0.5 * (amin + bmax); (*P)[i] = t; (*Q)[i] = t; } } } } return std::sqrt(result); } FCL_REAL AABB::distance(const AABB& other) const { FCL_REAL result = 0; for(std::size_t i = 0; i < 3; ++i) { const FCL_REAL& amin = min_[i]; const FCL_REAL& amax = max_[i]; const FCL_REAL& bmin = other.min_[i]; const FCL_REAL& bmax = other.max_[i]; if(amin > bmax) { FCL_REAL delta = bmax - amin; result += delta * delta; } else if(bmin > amax) { FCL_REAL delta = amax - bmin; result += delta * delta; } } return std::sqrt(result); } } fcl-0.5.0/src/BV/OBB.cpp000066400000000000000000000245001274356571000144770ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/BV/OBB.h" #include "fcl/BVH/BVH_utility.h" #include "fcl/math/transform.h" #include #include namespace fcl { /// @brief Compute the 8 vertices of a OBB inline void computeVertices(const OBB& b, Vec3f vertices[8]) { const Vec3f* axis = b.axis; const Vec3f& extent = b.extent; const Vec3f& To = b.To; Vec3f extAxis0 = axis[0] * extent[0]; Vec3f extAxis1 = axis[1] * extent[1]; Vec3f extAxis2 = axis[2] * extent[2]; vertices[0] = To - extAxis0 - extAxis1 - extAxis2; vertices[1] = To + extAxis0 - extAxis1 - extAxis2; vertices[2] = To + extAxis0 + extAxis1 - extAxis2; vertices[3] = To - extAxis0 + extAxis1 - extAxis2; vertices[4] = To - extAxis0 - extAxis1 + extAxis2; vertices[5] = To + extAxis0 - extAxis1 + extAxis2; vertices[6] = To + extAxis0 + extAxis1 + extAxis2; vertices[7] = To - extAxis0 + extAxis1 + extAxis2; } /// @brief OBB merge method when the centers of two smaller OBB are far away inline OBB merge_largedist(const OBB& b1, const OBB& b2) { OBB b; Vec3f vertex[16]; computeVertices(b1, vertex); computeVertices(b2, vertex + 8); Matrix3f M; Vec3f E[3]; Matrix3f::U s[3] = {0, 0, 0}; // obb axes Vec3f& R0 = b.axis[0]; Vec3f& R1 = b.axis[1]; Vec3f& R2 = b.axis[2]; R0 = b1.To - b2.To; R0.normalize(); Vec3f vertex_proj[16]; for(int i = 0; i < 16; ++i) vertex_proj[i] = vertex[i] - R0 * vertex[i].dot(R0); getCovariance(vertex_proj, NULL, NULL, NULL, 16, M); eigen(M, s, E); int min, mid, max; if (s[0] > s[1]) { max = 0; min = 1; } else { min = 0; max = 1; } if (s[2] < s[min]) { mid = min; min = 2; } else if (s[2] > s[max]) { mid = max; max = 2; } else { mid = 2; } R1.setValue(E[0][max], E[1][max], E[2][max]); R2.setValue(E[0][mid], E[1][mid], E[2][mid]); // set obb centers and extensions Vec3f center, extent; getExtentAndCenter(vertex, NULL, NULL, NULL, 16, b.axis, center, extent); b.To = center; b.extent = extent; return b; } /// @brief OBB merge method when the centers of two smaller OBB are close inline OBB merge_smalldist(const OBB& b1, const OBB& b2) { OBB b; b.To = (b1.To + b2.To) * 0.5; Quaternion3f q0, q1; q0.fromAxes(b1.axis); q1.fromAxes(b2.axis); if(q0.dot(q1) < 0) q1 = -q1; Quaternion3f q = q0 + q1; FCL_REAL inv_length = 1.0 / std::sqrt(q.dot(q)); q = q * inv_length; q.toAxes(b.axis); Vec3f vertex[8], diff; FCL_REAL real_max = std::numeric_limits::max(); Vec3f pmin(real_max, real_max, real_max); Vec3f pmax(-real_max, -real_max, -real_max); computeVertices(b1, vertex); for(int i = 0; i < 8; ++i) { diff = vertex[i] - b.To; for(int j = 0; j < 3; ++j) { FCL_REAL dot = diff.dot(b.axis[j]); if(dot > pmax[j]) pmax[j] = dot; else if(dot < pmin[j]) pmin[j] = dot; } } computeVertices(b2, vertex); for(int i = 0; i < 8; ++i) { diff = vertex[i] - b.To; for(int j = 0; j < 3; ++j) { FCL_REAL dot = diff.dot(b.axis[j]); if(dot > pmax[j]) pmax[j] = dot; else if(dot < pmin[j]) pmin[j] = dot; } } for(int j = 0; j < 3; ++j) { b.To += (b.axis[j] * (0.5 * (pmax[j] + pmin[j]))); b.extent[j] = 0.5 * (pmax[j] - pmin[j]); } return b; } bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b) { register FCL_REAL t, s; const FCL_REAL reps = 1e-6; Matrix3f Bf = abs(B); Bf += reps; // if any of these tests are one-sided, then the polyhedra are disjoint // A1 x A2 = A0 t = ((T[0] < 0.0) ? -T[0] : T[0]); if(t > (a[0] + Bf.dotX(b))) return true; // B1 x B2 = B0 s = B.transposeDotX(T); t = ((s < 0.0) ? -s : s); if(t > (b[0] + Bf.transposeDotX(a))) return true; // A2 x A0 = A1 t = ((T[1] < 0.0) ? -T[1] : T[1]); if(t > (a[1] + Bf.dotY(b))) return true; // A0 x A1 = A2 t =((T[2] < 0.0) ? -T[2] : T[2]); if(t > (a[2] + Bf.dotZ(b))) return true; // B2 x B0 = B1 s = B.transposeDotY(T); t = ((s < 0.0) ? -s : s); if(t > (b[1] + Bf.transposeDotY(a))) return true; // B0 x B1 = B2 s = B.transposeDotZ(T); t = ((s < 0.0) ? -s : s); if(t > (b[2] + Bf.transposeDotZ(a))) return true; // A0 x B0 s = T[2] * B(1, 0) - T[1] * B(2, 0); t = ((s < 0.0) ? -s : s); if(t > (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) + b[2] * Bf(0, 1))) return true; // A0 x B1 s = T[2] * B(1, 1) - T[1] * B(2, 1); t = ((s < 0.0) ? -s : s); if(t > (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + b[0] * Bf(0, 2) + b[2] * Bf(0, 0))) return true; // A0 x B2 s = T[2] * B(1, 2) - T[1] * B(2, 2); t = ((s < 0.0) ? -s : s); if(t > (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + b[0] * Bf(0, 1) + b[1] * Bf(0, 0))) return true; // A1 x B0 s = T[0] * B(2, 0) - T[2] * B(0, 0); t = ((s < 0.0) ? -s : s); if(t > (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + b[1] * Bf(1, 2) + b[2] * Bf(1, 1))) return true; // A1 x B1 s = T[0] * B(2, 1) - T[2] * B(0, 1); t = ((s < 0.0) ? -s : s); if(t > (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + b[0] * Bf(1, 2) + b[2] * Bf(1, 0))) return true; // A1 x B2 s = T[0] * B(2, 2) - T[2] * B(0, 2); t = ((s < 0.0) ? -s : s); if(t > (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + b[0] * Bf(1, 1) + b[1] * Bf(1, 0))) return true; // A2 x B0 s = T[1] * B(0, 0) - T[0] * B(1, 0); t = ((s < 0.0) ? -s : s); if(t > (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + b[1] * Bf(2, 2) + b[2] * Bf(2, 1))) return true; // A2 x B1 s = T[1] * B(0, 1) - T[0] * B(1, 1); t = ((s < 0.0) ? -s : s); if(t > (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + b[0] * Bf(2, 2) + b[2] * Bf(2, 0))) return true; // A2 x B2 s = T[1] * B(0, 2) - T[0] * B(1, 2); t = ((s < 0.0) ? -s : s); if(t > (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + b[0] * Bf(2, 1) + b[1] * Bf(2, 0))) return true; return false; } bool OBB::overlap(const OBB& other) const { /// compute what transform [R,T] that takes us from cs1 to cs2. /// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)] /// First compute the rotation part, then translation part Vec3f t = other.To - To; // T2 - T1 Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1) Matrix3f R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), axis[0].dot(other.axis[2]), axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), axis[1].dot(other.axis[2]), axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), axis[2].dot(other.axis[2])); return !obbDisjoint(R, T, extent, other.extent); } bool OBB::contain(const Vec3f& p) const { Vec3f local_p = p - To; FCL_REAL proj = local_p.dot(axis[0]); if((proj > extent[0]) || (proj < -extent[0])) return false; proj = local_p.dot(axis[1]); if((proj > extent[1]) || (proj < -extent[1])) return false; proj = local_p.dot(axis[2]); if((proj > extent[2]) || (proj < -extent[2])) return false; return true; } OBB& OBB::operator += (const Vec3f& p) { OBB bvp; bvp.To = p; bvp.axis[0] = axis[0]; bvp.axis[1] = axis[1]; bvp.axis[2] = axis[2]; bvp.extent.setValue(0); *this += bvp; return *this; } OBB OBB::operator + (const OBB& other) const { Vec3f center_diff = To - other.To; FCL_REAL max_extent = std::max(std::max(extent[0], extent[1]), extent[2]); FCL_REAL max_extent2 = std::max(std::max(other.extent[0], other.extent[1]), other.extent[2]); if(center_diff.length() > 2 * (max_extent + max_extent2)) { return merge_largedist(*this, other); } else { return merge_smalldist(*this, other); } } FCL_REAL OBB::distance(const OBB& other, Vec3f* P, Vec3f* Q) const { std::cerr << "OBB distance not implemented!" << std::endl; return 0.0; } bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2) { Matrix3f R0b2(R0.dotX(b2.axis[0]), R0.dotX(b2.axis[1]), R0.dotX(b2.axis[2]), R0.dotY(b2.axis[0]), R0.dotY(b2.axis[1]), R0.dotY(b2.axis[2]), R0.dotZ(b2.axis[0]), R0.dotZ(b2.axis[1]), R0.dotZ(b2.axis[2])); Matrix3f R(R0b2.transposeDotX(b1.axis[0]), R0b2.transposeDotY(b1.axis[0]), R0b2.transposeDotZ(b1.axis[0]), R0b2.transposeDotX(b1.axis[1]), R0b2.transposeDotY(b1.axis[1]), R0b2.transposeDotZ(b1.axis[1]), R0b2.transposeDotX(b1.axis[2]), R0b2.transposeDotY(b1.axis[2]), R0b2.transposeDotZ(b1.axis[2])); Vec3f Ttemp = R0 * b2.To + T0 - b1.To; Vec3f T(Ttemp.dot(b1.axis[0]), Ttemp.dot(b1.axis[1]), Ttemp.dot(b1.axis[2])); return !obbDisjoint(R, T, b1.extent, b2.extent); } OBB translate(const OBB& bv, const Vec3f& t) { OBB res(bv); res.To += t; return res; } } fcl-0.5.0/src/BV/OBBRSS.cpp000066400000000000000000000043031274356571000150660ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/BV/OBBRSS.h" namespace fcl { bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2) { return overlap(R0, T0, b1.obb, b2.obb); } FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2, Vec3f* P, Vec3f* Q) { return distance(R0, T0, b1.rss, b2.rss, P, Q); } OBBRSS translate(const OBBRSS& bv, const Vec3f& t) { OBBRSS res(bv); res.obb.To += t; res.rss.Tr += t; return res; } } fcl-0.5.0/src/BV/RSS.cpp000066400000000000000000000732421274356571000145530ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/BV/RSS.h" #include "fcl/BVH/BVH_utility.h" #include namespace fcl { /// @brief Clip value between a and b void clipToRange(FCL_REAL& val, FCL_REAL a, FCL_REAL b) { if(val < a) val = a; else if(val > b) val = b; } /// @brief Finds the parameters t & u corresponding to the two closest points on a pair of line segments. /// The first segment is defined as Pa + A*t, 0 <= t <= a, where "Pa" is one endpoint of the segment, "A" is a unit vector /// pointing to the other endpoint, and t is a scalar that produces all the points between the two endpoints. Since "A" is a unit /// vector, "a" is the segment's length. /// The second segment is defined as Pb + B*u, 0 <= u <= b. /// Many of the terms needed by the algorithm are already computed for other purposes,so we just pass these terms into the function /// instead of complete specifications of each segment. "T" in the dot products is the vector betweeen Pa and Pb. /// Reference: "On fast computation of distance between line segments." Vladimir J. Lumelsky, in Information Processing Letters, no. 21, pages 55-61, 1985. void segCoords(FCL_REAL& t, FCL_REAL& u, FCL_REAL a, FCL_REAL b, FCL_REAL A_dot_B, FCL_REAL A_dot_T, FCL_REAL B_dot_T) { FCL_REAL denom = 1 - A_dot_B * A_dot_B; if(denom == 0) t = 0; else { t = (A_dot_T - B_dot_T * A_dot_B) / denom; clipToRange(t, 0, a); } u = t * A_dot_B - B_dot_T; if(u < 0) { u = 0; t = A_dot_T; clipToRange(t, 0, a); } else if(u > b) { u = b; t = u * A_dot_B + A_dot_T; clipToRange(t, 0, a); } } /// @brief Returns whether the nearest point on rectangle edge /// Pb + B*u, 0 <= u <= b, to the rectangle edge, /// Pa + A*t, 0 <= t <= a, is within the half space /// determined by the point Pa and the direction Anorm. /// A,B, and Anorm are unit vectors. T is the vector between Pa and Pb. bool inVoronoi(FCL_REAL a, FCL_REAL b, FCL_REAL Anorm_dot_B, FCL_REAL Anorm_dot_T, FCL_REAL A_dot_B, FCL_REAL A_dot_T, FCL_REAL B_dot_T) { if(fabs(Anorm_dot_B) < 1e-7) return false; FCL_REAL t, u, v; u = -Anorm_dot_T / Anorm_dot_B; clipToRange(u, 0, b); t = u * A_dot_B + A_dot_T; clipToRange(t, 0, a); v = t * A_dot_B - B_dot_T; if(Anorm_dot_B > 0) { if(v > (u + 1e-7)) return true; } else { if(v < (u - 1e-7)) return true; } return false; } /// @brief Distance between two oriented rectangles; P and Q (optional return values) are the closest points in the rectangles, both are in the local frame of the first rectangle. FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2], const FCL_REAL b[2], Vec3f* P = NULL, Vec3f* Q = NULL) { FCL_REAL A0_dot_B0, A0_dot_B1, A1_dot_B0, A1_dot_B1; A0_dot_B0 = Rab(0, 0); A0_dot_B1 = Rab(0, 1); A1_dot_B0 = Rab(1, 0); A1_dot_B1 = Rab(1, 1); FCL_REAL aA0_dot_B0, aA0_dot_B1, aA1_dot_B0, aA1_dot_B1; FCL_REAL bA0_dot_B0, bA0_dot_B1, bA1_dot_B0, bA1_dot_B1; aA0_dot_B0 = a[0] * A0_dot_B0; aA0_dot_B1 = a[0] * A0_dot_B1; aA1_dot_B0 = a[1] * A1_dot_B0; aA1_dot_B1 = a[1] * A1_dot_B1; bA0_dot_B0 = b[0] * A0_dot_B0; bA1_dot_B0 = b[0] * A1_dot_B0; bA0_dot_B1 = b[1] * A0_dot_B1; bA1_dot_B1 = b[1] * A1_dot_B1; Vec3f Tba = Rab.transposeTimes(Tab); Vec3f S; FCL_REAL t, u; // determine if any edge pair contains the closest points FCL_REAL ALL_x, ALU_x, AUL_x, AUU_x; FCL_REAL BLL_x, BLU_x, BUL_x, BUU_x; FCL_REAL LA1_lx, LA1_ux, UA1_lx, UA1_ux, LB1_lx, LB1_ux, UB1_lx, UB1_ux; ALL_x = -Tba[0]; ALU_x = ALL_x + aA1_dot_B0; AUL_x = ALL_x + aA0_dot_B0; AUU_x = ALU_x + aA0_dot_B0; if(ALL_x < ALU_x) { LA1_lx = ALL_x; LA1_ux = ALU_x; UA1_lx = AUL_x; UA1_ux = AUU_x; } else { LA1_lx = ALU_x; LA1_ux = ALL_x; UA1_lx = AUU_x; UA1_ux = AUL_x; } BLL_x = Tab[0]; BLU_x = BLL_x + bA0_dot_B1; BUL_x = BLL_x + bA0_dot_B0; BUU_x = BLU_x + bA0_dot_B0; if(BLL_x < BLU_x) { LB1_lx = BLL_x; LB1_ux = BLU_x; UB1_lx = BUL_x; UB1_ux = BUU_x; } else { LB1_lx = BLU_x; LB1_ux = BLL_x; UB1_lx = BUU_x; UB1_ux = BUL_x; } // UA1, UB1 if((UA1_ux > b[0]) && (UB1_ux > a[0])) { if(((UA1_lx > b[0]) || inVoronoi(b[1], a[1], A1_dot_B0, aA0_dot_B0 - b[0] - Tba[0], A1_dot_B1, aA0_dot_B1 - Tba[1], -Tab[1] - bA1_dot_B0)) && ((UB1_lx > a[0]) || inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0 - a[0], A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1] - aA0_dot_B1))) { segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1] - aA0_dot_B1); S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - a[0] ; S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - t; S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u; if(P && Q) { P->setValue(a[0], t, 0); *Q = S + (*P); } return S.length(); } } // UA1, LB1 if((UA1_lx < 0) && (LB1_ux > a[0])) { if(((UA1_ux < 0) || inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0] - aA0_dot_B0, A1_dot_B1, aA0_dot_B1 - Tba[1], -Tab[1])) && ((LB1_lx > a[0]) || inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] - a[0], A1_dot_B1, Tab[1], Tba[1] - aA0_dot_B1))) { segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1], Tba[1] - aA0_dot_B1); S[0] = Tab[0] + Rab(0, 1) * u - a[0]; S[1] = Tab[1] + Rab(1, 1) * u - t; S[2] = Tab[2] + Rab(2, 1) * u; if(P && Q) { P->setValue(a[0], t, 0); *Q = S + (*P); } return S.length(); } } // LA1, UB1 if((LA1_ux > b[0]) && (UB1_lx < 0)) { if(((LA1_lx > b[0]) || inVoronoi(b[1], a[1], A1_dot_B0, -Tba[0] - b[0], A1_dot_B1, -Tba[1], -Tab[1] - bA1_dot_B0)) && ((UB1_ux < 0) || inVoronoi(a[1], b[1], -A0_dot_B1, -Tab[0] - bA0_dot_B0, A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1]))) { segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1]); S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u; S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - t; S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u; if(P && Q) { P->setValue(0, t, 0); *Q = S + (*P); } return S.length(); } } // LA1, LB1 if((LA1_lx < 0) && (LB1_lx < 0)) { if (((LA1_ux < 0) || inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0], A1_dot_B1, -Tba[1], -Tab[1])) && ((LB1_ux < 0) || inVoronoi(a[1], b[1], -A0_dot_B1, -Tab[0], A1_dot_B1, Tab[1], Tba[1]))) { segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1], Tba[1]); S[0] = Tab[0] + Rab(0, 1) * u; S[1] = Tab[1] + Rab(1, 1) * u - t; S[2] = Tab[2] + Rab(2, 1) * u; if(P && Q) { P->setValue(0, t, 0); *Q = S + (*P); } return S.length(); } } FCL_REAL ALL_y, ALU_y, AUL_y, AUU_y; ALL_y = -Tba[1]; ALU_y = ALL_y + aA1_dot_B1; AUL_y = ALL_y + aA0_dot_B1; AUU_y = ALU_y + aA0_dot_B1; FCL_REAL LA1_ly, LA1_uy, UA1_ly, UA1_uy, LB0_lx, LB0_ux, UB0_lx, UB0_ux; if(ALL_y < ALU_y) { LA1_ly = ALL_y; LA1_uy = ALU_y; UA1_ly = AUL_y; UA1_uy = AUU_y; } else { LA1_ly = ALU_y; LA1_uy = ALL_y; UA1_ly = AUU_y; UA1_uy = AUL_y; } if(BLL_x < BUL_x) { LB0_lx = BLL_x; LB0_ux = BUL_x; UB0_lx = BLU_x; UB0_ux = BUU_x; } else { LB0_lx = BUL_x; LB0_ux = BLL_x; UB0_lx = BUU_x; UB0_ux = BLU_x; } // UA1, UB0 if((UA1_uy > b[1]) && (UB0_ux > a[0])) { if(((UA1_ly > b[1]) || inVoronoi(b[0], a[1], A1_dot_B1, aA0_dot_B1 - Tba[1] - b[1], A1_dot_B0, aA0_dot_B0 - Tba[0], -Tab[1] - bA1_dot_B1)) && ((UB0_lx > a[0]) || inVoronoi(a[1], b[0], A0_dot_B0, Tab[0] - a[0] + bA0_dot_B1, A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0] - aA0_dot_B0))) { segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0] - aA0_dot_B0); S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - a[0] ; S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - t; S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u; if(P && Q) { P->setValue(a[0], t, 0); *Q = S + (*P); } return S.length(); } } // UA1, LB0 if((UA1_ly < 0) && (LB0_ux > a[0])) { if(((UA1_uy < 0) || inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1] - aA0_dot_B1, A1_dot_B0, aA0_dot_B0 - Tba[0], -Tab[1])) && ((LB0_lx > a[0]) || inVoronoi(a[1], b[0], A0_dot_B0, Tab[0] - a[0], A1_dot_B0, Tab[1], Tba[0] - aA0_dot_B0))) { segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1], Tba[0] - aA0_dot_B0); S[0] = Tab[0] + Rab(0, 0) * u - a[0]; S[1] = Tab[1] + Rab(1, 0) * u - t; S[2] = Tab[2] + Rab(2, 0) * u; if(P && Q) { P->setValue(a[0], t, 0); *Q = S + (*P); } return S.length(); } } // LA1, UB0 if((LA1_uy > b[1]) && (UB0_lx < 0)) { if(((LA1_ly > b[1]) || inVoronoi(b[0], a[1], A1_dot_B1, -Tba[1] - b[1], A1_dot_B0, -Tba[0], -Tab[1] - bA1_dot_B1)) && ((UB0_ux < 0) || inVoronoi(a[1], b[0], -A0_dot_B0, -Tab[0] - bA0_dot_B1, A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0]))) { segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0]); S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u; S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - t; S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u; if(P && Q) { P->setValue(0, t, 0); *Q = S + (*P); } return S.length(); } } // LA1, LB0 if((LA1_ly < 0) && (LB0_lx < 0)) { if(((LA1_uy < 0) || inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1], A1_dot_B0, -Tba[0], -Tab[1])) && ((LB0_ux < 0) || inVoronoi(a[1], b[0], -A0_dot_B0, -Tab[0], A1_dot_B0, Tab[1], Tba[0]))) { segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1], Tba[0]); S[0] = Tab[0] + Rab(0, 0) * u; S[1] = Tab[1] + Rab(1, 0) * u - t; S[2] = Tab[2] + Rab(2, 0) * u; if(P&& Q) { P->setValue(0, t, 0); *Q = S + (*P); } return S.length(); } } FCL_REAL BLL_y, BLU_y, BUL_y, BUU_y; BLL_y = Tab[1]; BLU_y = BLL_y + bA1_dot_B1; BUL_y = BLL_y + bA1_dot_B0; BUU_y = BLU_y + bA1_dot_B0; FCL_REAL LA0_lx, LA0_ux, UA0_lx, UA0_ux, LB1_ly, LB1_uy, UB1_ly, UB1_uy; if(ALL_x < AUL_x) { LA0_lx = ALL_x; LA0_ux = AUL_x; UA0_lx = ALU_x; UA0_ux = AUU_x; } else { LA0_lx = AUL_x; LA0_ux = ALL_x; UA0_lx = AUU_x; UA0_ux = ALU_x; } if(BLL_y < BLU_y) { LB1_ly = BLL_y; LB1_uy = BLU_y; UB1_ly = BUL_y; UB1_uy = BUU_y; } else { LB1_ly = BLU_y; LB1_uy = BLL_y; UB1_ly = BUU_y; UB1_uy = BUL_y; } // UA0, UB1 if((UA0_ux > b[0]) && (UB1_uy > a[1])) { if(((UA0_lx > b[0]) || inVoronoi(b[1], a[0], A0_dot_B0, aA1_dot_B0 - Tba[0] - b[0], A0_dot_B1, aA1_dot_B1 - Tba[1], -Tab[0] - bA0_dot_B0)) && ((UB1_ly > a[1]) || inVoronoi(a[0], b[1], A1_dot_B1, Tab[1] - a[1] + bA1_dot_B0, A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1] - aA1_dot_B1))) { segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1] - aA1_dot_B1); S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - t; S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - a[1]; S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u; if(P && Q) { P->setValue(t, a[1], 0); *Q = S + (*P); } return S.length(); } } // UA0, LB1 if((UA0_lx < 0) && (LB1_uy > a[1])) { if(((UA0_ux < 0) || inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0] - aA1_dot_B0, A0_dot_B1, aA1_dot_B1 - Tba[1], -Tab[0])) && ((LB1_ly > a[1]) || inVoronoi(a[0], b[1], A1_dot_B1, Tab[1] - a[1], A0_dot_B1, Tab[0], Tba[1] - aA1_dot_B1))) { segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0], Tba[1] - aA1_dot_B1); S[0] = Tab[0] + Rab(0, 1) * u - t; S[1] = Tab[1] + Rab(1, 1) * u - a[1]; S[2] = Tab[2] + Rab(2, 1) * u; if(P && Q) { P->setValue(t, a[1], 0); *Q = S + (*P); } return S.length(); } } // LA0, UB1 if((LA0_ux > b[0]) && (UB1_ly < 0)) { if(((LA0_lx > b[0]) || inVoronoi(b[1], a[0], A0_dot_B0, -b[0] - Tba[0], A0_dot_B1, -Tba[1], -bA0_dot_B0 - Tab[0])) && ((UB1_uy < 0) || inVoronoi(a[0], b[1], -A1_dot_B1, -Tab[1] - bA1_dot_B0, A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1]))) { segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1]); S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - t; S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u; S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u; if(P && Q) { P->setValue(t, 0, 0); *Q = S + (*P); } return S.length(); } } // LA0, LB1 if((LA0_lx < 0) && (LB1_ly < 0)) { if(((LA0_ux < 0) || inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0], A0_dot_B1, -Tba[1], -Tab[0])) && ((LB1_uy < 0) || inVoronoi(a[0], b[1], -A1_dot_B1, -Tab[1], A0_dot_B1, Tab[0], Tba[1]))) { segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0], Tba[1]); S[0] = Tab[0] + Rab(0, 1) * u - t; S[1] = Tab[1] + Rab(1, 1) * u; S[2] = Tab[2] + Rab(2, 1) * u; if(P && Q) { P->setValue(t, 0, 0); *Q = S + (*P); } return S.length(); } } FCL_REAL LA0_ly, LA0_uy, UA0_ly, UA0_uy, LB0_ly, LB0_uy, UB0_ly, UB0_uy; if(ALL_y < AUL_y) { LA0_ly = ALL_y; LA0_uy = AUL_y; UA0_ly = ALU_y; UA0_uy = AUU_y; } else { LA0_ly = AUL_y; LA0_uy = ALL_y; UA0_ly = AUU_y; UA0_uy = ALU_y; } if(BLL_y < BUL_y) { LB0_ly = BLL_y; LB0_uy = BUL_y; UB0_ly = BLU_y; UB0_uy = BUU_y; } else { LB0_ly = BUL_y; LB0_uy = BLL_y; UB0_ly = BUU_y; UB0_uy = BLU_y; } // UA0, UB0 if((UA0_uy > b[1]) && (UB0_uy > a[1])) { if(((UA0_ly > b[1]) || inVoronoi(b[0], a[0], A0_dot_B1, aA1_dot_B1 - Tba[1] - b[1], A0_dot_B0, aA1_dot_B0 - Tba[0], -Tab[0] - bA0_dot_B1)) && ((UB0_ly > a[1]) || inVoronoi(a[0], b[0], A1_dot_B0, Tab[1] - a[1] + bA1_dot_B1, A0_dot_B0, Tab[0] + bA0_dot_B1, Tba[0] - aA1_dot_B0))) { segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0] + bA0_dot_B1, Tba[0] - aA1_dot_B0); S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - t; S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - a[1]; S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u; if(P && Q) { P->setValue(t, a[1], 0); *Q = S + (*P); } return S.length(); } } // UA0, LB0 if((UA0_ly < 0) && (LB0_uy > a[1])) { if(((UA0_uy < 0) || inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1] - aA1_dot_B1, A0_dot_B0, aA1_dot_B0 - Tba[0], -Tab[0])) && ((LB0_ly > a[1]) || inVoronoi(a[0], b[0], A1_dot_B0, Tab[1] - a[1], A0_dot_B0, Tab[0], Tba[0] - aA1_dot_B0))) { segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0], Tba[0] - aA1_dot_B0); S[0] = Tab[0] + Rab(0, 0) * u - t; S[1] = Tab[1] + Rab(1, 0) * u - a[1]; S[2] = Tab[2] + Rab(2, 0) * u; if(P && Q) { P->setValue(t, a[1], 0); *Q = S + (*P); } return S.length(); } } // LA0, UB0 if((LA0_uy > b[1]) && (UB0_ly < 0)) { if(((LA0_ly > b[1]) || inVoronoi(b[0], a[0], A0_dot_B1, -Tba[1] - b[1], A0_dot_B0, -Tba[0], -Tab[0] - bA0_dot_B1)) && ((UB0_uy < 0) || inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1] - bA1_dot_B1, A0_dot_B0, Tab[0] + bA0_dot_B1, Tba[0]))) { segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0] + bA0_dot_B1, Tba[0]); S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - t; S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u; S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u; if(P && Q) { P->setValue(t, 0, 0); *Q = S + (*P); } return S.length(); } } // LA0, LB0 if((LA0_ly < 0) && (LB0_ly < 0)) { if(((LA0_uy < 0) || inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1], A0_dot_B0, -Tba[0], -Tab[0])) && ((LB0_uy < 0) || inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1], A0_dot_B0, Tab[0], Tba[0]))) { segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0], Tba[0]); S[0] = Tab[0] + Rab(0, 0) * u - t; S[1] = Tab[1] + Rab(1, 0) * u; S[2] = Tab[2] + Rab(2, 0) * u; if(P && Q) { P->setValue(t, 0, 0); *Q = S + (*P); } return S.length(); } } // no edges passed, take max separation along face normals FCL_REAL sep1, sep2; if(Tab[2] > 0.0) { sep1 = Tab[2]; if (Rab(2, 0) < 0.0) sep1 += b[0] * Rab(2, 0); if (Rab(2, 1) < 0.0) sep1 += b[1] * Rab(2, 1); } else { sep1 = -Tab[2]; if (Rab(2, 0) > 0.0) sep1 -= b[0] * Rab(2, 0); if (Rab(2, 1) > 0.0) sep1 -= b[1] * Rab(2, 1); } if(Tba[2] < 0) { sep2 = -Tba[2]; if (Rab(0, 2) < 0.0) sep2 += a[0] * Rab(0, 2); if (Rab(1, 2) < 0.0) sep2 += a[1] * Rab(1, 2); } else { sep2 = Tba[2]; if (Rab(0, 2) > 0.0) sep2 -= a[0] * Rab(0, 2); if (Rab(1, 2) > 0.0) sep2 -= a[1] * Rab(1, 2); } if(sep1 >= sep2 && sep1 >= 0) { if(Tab[2] > 0) S.setValue(0, 0, sep1); else S.setValue(0, 0, -sep1); if(P && Q) { *Q = S; P->setValue(0); } } if(sep2 >= sep1 && sep2 >= 0) { Vec3f Q_(Tab[0], Tab[1], Tab[2]); Vec3f P_; if(Tba[2] < 0) { P_[0] = Rab(0, 2) * sep2 + Tab[0]; P_[1] = Rab(1, 2) * sep2 + Tab[1]; P_[2] = Rab(2, 2) * sep2 + Tab[2]; } else { P_[0] = -Rab(0, 2) * sep2 + Tab[0]; P_[1] = -Rab(1, 2) * sep2 + Tab[1]; P_[2] = -Rab(2, 2) * sep2 + Tab[2]; } S = Q_ - P_; if(P && Q) { *P = P_; *Q = Q_; } } FCL_REAL sep = (sep1 > sep2 ? sep1 : sep2); return (sep > 0 ? sep : 0); } bool RSS::overlap(const RSS& other) const { /// compute what transform [R,T] that takes us from cs1 to cs2. /// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)] /// First compute the rotation part, then translation part /// First compute T2 - T1 Vec3f t = other.Tr - Tr; /// Then compute R1'(T2 - T1) Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); /// Now compute R1'R2 Matrix3f R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), axis[0].dot(other.axis[2]), axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), axis[1].dot(other.axis[2]), axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), axis[2].dot(other.axis[2])); FCL_REAL dist = rectDistance(R, T, l, other.l); return (dist <= (r + other.r)); } bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2) { Matrix3f R0b2(R0.dotX(b2.axis[0]), R0.dotX(b2.axis[1]), R0.dotX(b2.axis[2]), R0.dotY(b2.axis[0]), R0.dotY(b2.axis[1]), R0.dotY(b2.axis[2]), R0.dotZ(b2.axis[0]), R0.dotZ(b2.axis[1]), R0.dotZ(b2.axis[2])); Matrix3f R(R0b2.transposeDotX(b1.axis[0]), R0b2.transposeDotY(b1.axis[0]), R0b2.transposeDotZ(b1.axis[0]), R0b2.transposeDotX(b1.axis[1]), R0b2.transposeDotY(b1.axis[1]), R0b2.transposeDotZ(b1.axis[1]), R0b2.transposeDotX(b1.axis[2]), R0b2.transposeDotY(b1.axis[2]), R0b2.transposeDotZ(b1.axis[2])); Vec3f Ttemp = R0 * b2.Tr + T0 - b1.Tr; Vec3f T(Ttemp.dot(b1.axis[0]), Ttemp.dot(b1.axis[1]), Ttemp.dot(b1.axis[2])); FCL_REAL dist = rectDistance(R, T, b1.l, b2.l); return (dist <= (b1.r + b2.r)); } bool RSS::contain(const Vec3f& p) const { Vec3f local_p = p - Tr; FCL_REAL proj0 = local_p.dot(axis[0]); FCL_REAL proj1 = local_p.dot(axis[1]); FCL_REAL proj2 = local_p.dot(axis[2]); FCL_REAL abs_proj2 = fabs(proj2); Vec3f proj(proj0, proj1, proj2); /// projection is within the rectangle if((proj0 < l[0]) && (proj0 > 0) && (proj1 < l[1]) && (proj1 > 0)) { return (abs_proj2 < r); } else if((proj0 < l[0]) && (proj0 > 0) && ((proj1 < 0) || (proj1 > l[1]))) { FCL_REAL y = (proj1 > 0) ? l[1] : 0; Vec3f v(proj0, y, 0); return ((proj - v).sqrLength() < r * r); } else if((proj1 < l[1]) && (proj1 > 0) && ((proj0 < 0) || (proj0 > l[0]))) { FCL_REAL x = (proj0 > 0) ? l[0] : 0; Vec3f v(x, proj1, 0); return ((proj - v).sqrLength() < r * r); } else { FCL_REAL x = (proj0 > 0) ? l[0] : 0; FCL_REAL y = (proj1 > 0) ? l[1] : 0; Vec3f v(x, y, 0); return ((proj - v).sqrLength() < r * r); } } RSS& RSS::operator += (const Vec3f& p) { Vec3f local_p = p - Tr; FCL_REAL proj0 = local_p.dot(axis[0]); FCL_REAL proj1 = local_p.dot(axis[1]); FCL_REAL proj2 = local_p.dot(axis[2]); FCL_REAL abs_proj2 = fabs(proj2); Vec3f proj(proj0, proj1, proj2); // projection is within the rectangle if((proj0 < l[0]) && (proj0 > 0) && (proj1 < l[1]) && (proj1 > 0)) { if(abs_proj2 < r) ; // do nothing else { r = 0.5 * (r + abs_proj2); // enlarge the r // change RSS origin position if(proj2 > 0) Tr[2] += 0.5 * (abs_proj2 - r); else Tr[2] -= 0.5 * (abs_proj2 - r); } } else if((proj0 < l[0]) && (proj0 > 0) && ((proj1 < 0) || (proj1 > l[1]))) { FCL_REAL y = (proj1 > 0) ? l[1] : 0; Vec3f v(proj0, y, 0); FCL_REAL new_r_sqr = (proj - v).sqrLength(); if(new_r_sqr < r * r) ; // do nothing else { if(abs_proj2 < r) { FCL_REAL delta_y = - std::sqrt(r * r - proj2 * proj2) + fabs(proj1 - y); l[1] += delta_y; if(proj1 < 0) Tr[1] -= delta_y; } else { FCL_REAL delta_y = fabs(proj1 - y); l[1] += delta_y; if(proj1 < 0) Tr[1] -= delta_y; if(proj2 > 0) Tr[2] += 0.5 * (abs_proj2 - r); else Tr[2] -= 0.5 * (abs_proj2 - r); } } } else if((proj1 < l[1]) && (proj1 > 0) && ((proj0 < 0) || (proj0 > l[0]))) { FCL_REAL x = (proj0 > 0) ? l[0] : 0; Vec3f v(x, proj1, 0); FCL_REAL new_r_sqr = (proj - v).sqrLength(); if(new_r_sqr < r * r) ; // do nothing else { if(abs_proj2 < r) { FCL_REAL delta_x = - std::sqrt(r * r - proj2 * proj2) + fabs(proj0 - x); l[0] += delta_x; if(proj0 < 0) Tr[0] -= delta_x; } else { FCL_REAL delta_x = fabs(proj0 - x); l[0] += delta_x; if(proj0 < 0) Tr[0] -= delta_x; if(proj2 > 0) Tr[2] += 0.5 * (abs_proj2 - r); else Tr[2] -= 0.5 * (abs_proj2 - r); } } } else { FCL_REAL x = (proj0 > 0) ? l[0] : 0; FCL_REAL y = (proj1 > 0) ? l[1] : 0; Vec3f v(x, y, 0); FCL_REAL new_r_sqr = (proj - v).sqrLength(); if(new_r_sqr < r * r) ; // do nothing else { if(abs_proj2 < r) { FCL_REAL diag = std::sqrt(new_r_sqr - proj2 * proj2); FCL_REAL delta_diag = - std::sqrt(r * r - proj2 * proj2) + diag; FCL_REAL delta_x = delta_diag / diag * fabs(proj0 - x); FCL_REAL delta_y = delta_diag / diag * fabs(proj1 - y); l[0] += delta_x; l[1] += delta_y; if(proj0 < 0 && proj1 < 0) { Tr[0] -= delta_x; Tr[1] -= delta_y; } } else { FCL_REAL delta_x = fabs(proj0 - x); FCL_REAL delta_y = fabs(proj1 - y); l[0] += delta_x; l[1] += delta_y; if(proj0 < 0 && proj1 < 0) { Tr[0] -= delta_x; Tr[1] -= delta_y; } if(proj2 > 0) Tr[2] += 0.5 * (abs_proj2 - r); else Tr[2] -= 0.5 * (abs_proj2 - r); } } } return *this; } RSS RSS::operator + (const RSS& other) const { RSS bv; Vec3f v[16]; Vec3f d0_pos = other.axis[0] * (other.l[0] + other.r); Vec3f d1_pos = other.axis[1] * (other.l[1] + other.r); Vec3f d0_neg = other.axis[0] * (-other.r); Vec3f d1_neg = other.axis[1] * (-other.r); Vec3f d2_pos = other.axis[2] * other.r; Vec3f d2_neg = other.axis[2] * (-other.r); v[0] = other.Tr + d0_pos + d1_pos + d2_pos; v[1] = other.Tr + d0_pos + d1_pos + d2_neg; v[2] = other.Tr + d0_pos + d1_neg + d2_pos; v[3] = other.Tr + d0_pos + d1_neg + d2_neg; v[4] = other.Tr + d0_neg + d1_pos + d2_pos; v[5] = other.Tr + d0_neg + d1_pos + d2_neg; v[6] = other.Tr + d0_neg + d1_neg + d2_pos; v[7] = other.Tr + d0_neg + d1_neg + d2_neg; d0_pos = axis[0] * (l[0] + r); d1_pos = axis[1] * (l[1] + r); d0_neg = axis[0] * (-r); d1_neg = axis[1] * (-r); d2_pos = axis[2] * r; d2_neg = axis[2] * (-r); v[8] = Tr + d0_pos + d1_pos + d2_pos; v[9] = Tr + d0_pos + d1_pos + d2_neg; v[10] = Tr + d0_pos + d1_neg + d2_pos; v[11] = Tr + d0_pos + d1_neg + d2_neg; v[12] = Tr + d0_neg + d1_pos + d2_pos; v[13] = Tr + d0_neg + d1_pos + d2_neg; v[14] = Tr + d0_neg + d1_neg + d2_pos; v[15] = Tr + d0_neg + d1_neg + d2_neg; Matrix3f M; // row first matrix Vec3f E[3]; // row first eigen-vectors Matrix3f::U s[3] = {0, 0, 0}; getCovariance(v, NULL, NULL, NULL, 16, M); eigen(M, s, E); int min, mid, max; if(s[0] > s[1]) { max = 0; min = 1; } else { min = 0; max = 1; } if(s[2] < s[min]) { mid = min; min = 2; } else if(s[2] > s[max]) { mid = max; max = 2; } else { mid = 2; } // column first matrix, as the axis in RSS bv.axis[0].setValue(E[0][max], E[1][max], E[2][max]); bv.axis[1].setValue(E[0][mid], E[1][mid], E[2][mid]); bv.axis[2].setValue(E[1][max]*E[2][mid] - E[1][mid]*E[2][max], E[0][mid]*E[2][max] - E[0][max]*E[2][mid], E[0][max]*E[1][mid] - E[0][mid]*E[1][max]); // set rss origin, rectangle size and radius getRadiusAndOriginAndRectangleSize(v, NULL, NULL, NULL, 16, bv.axis, bv.Tr, bv.l, bv.r); return bv; } FCL_REAL RSS::distance(const RSS& other, Vec3f* P, Vec3f* Q) const { // compute what transform [R,T] that takes us from cs1 to cs2. // [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)] // First compute the rotation part, then translation part Vec3f t = other.Tr - Tr; // T2 - T1 Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1) Matrix3f R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), axis[0].dot(other.axis[2]), axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), axis[1].dot(other.axis[2]), axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), axis[2].dot(other.axis[2])); FCL_REAL dist = rectDistance(R, T, l, other.l, P, Q); dist -= (r + other.r); return (dist < (FCL_REAL)0.0) ? (FCL_REAL)0.0 : dist; } FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2, Vec3f* P, Vec3f* Q) { Matrix3f R0b2(R0.dotX(b2.axis[0]), R0.dotX(b2.axis[1]), R0.dotX(b2.axis[2]), R0.dotY(b2.axis[0]), R0.dotY(b2.axis[1]), R0.dotY(b2.axis[2]), R0.dotZ(b2.axis[0]), R0.dotZ(b2.axis[1]), R0.dotZ(b2.axis[2])); Matrix3f R(R0b2.transposeDotX(b1.axis[0]), R0b2.transposeDotY(b1.axis[0]), R0b2.transposeDotZ(b1.axis[0]), R0b2.transposeDotX(b1.axis[1]), R0b2.transposeDotY(b1.axis[1]), R0b2.transposeDotZ(b1.axis[1]), R0b2.transposeDotX(b1.axis[2]), R0b2.transposeDotY(b1.axis[2]), R0b2.transposeDotZ(b1.axis[2])); Vec3f Ttemp = R0 * b2.Tr + T0 - b1.Tr; Vec3f T(Ttemp.dot(b1.axis[0]), Ttemp.dot(b1.axis[1]), Ttemp.dot(b1.axis[2])); FCL_REAL dist = rectDistance(R, T, b1.l, b2.l, P, Q); dist -= (b1.r + b2.r); return (dist < (FCL_REAL)0.0) ? (FCL_REAL)0.0 : dist; } RSS translate(const RSS& bv, const Vec3f& t) { RSS res(bv); res.Tr += t; return res; } } fcl-0.5.0/src/BV/kDOP.cpp000066400000000000000000000141611274356571000146740ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/BV/kDOP.h" #include #include namespace fcl { /// @brief Find the smaller and larger one of two values inline void minmax(FCL_REAL a, FCL_REAL b, FCL_REAL& minv, FCL_REAL& maxv) { if(a > b) { minv = b; maxv = a; } else { minv = a; maxv = b; } } /// @brief Merge the interval [minv, maxv] and value p/ inline void minmax(FCL_REAL p, FCL_REAL& minv, FCL_REAL& maxv) { if(p > maxv) maxv = p; if(p < minv) minv = p; } /// @brief Compute the distances to planes with normals from KDOP vectors except those of AABB face planes template void getDistances(const Vec3f& p, FCL_REAL* d) {} /// @brief Specification of getDistances template<> inline void getDistances<5>(const Vec3f& p, FCL_REAL* d) { d[0] = p[0] + p[1]; d[1] = p[0] + p[2]; d[2] = p[1] + p[2]; d[3] = p[0] - p[1]; d[4] = p[0] - p[2]; } template<> inline void getDistances<6>(const Vec3f& p, FCL_REAL* d) { d[0] = p[0] + p[1]; d[1] = p[0] + p[2]; d[2] = p[1] + p[2]; d[3] = p[0] - p[1]; d[4] = p[0] - p[2]; d[5] = p[1] - p[2]; } template<> inline void getDistances<9>(const Vec3f& p, FCL_REAL* d) { d[0] = p[0] + p[1]; d[1] = p[0] + p[2]; d[2] = p[1] + p[2]; d[3] = p[0] - p[1]; d[4] = p[0] - p[2]; d[5] = p[1] - p[2]; d[6] = p[0] + p[1] - p[2]; d[7] = p[0] + p[2] - p[1]; d[8] = p[1] + p[2] - p[0]; } template KDOP::KDOP() { FCL_REAL real_max = std::numeric_limits::max(); for(size_t i = 0; i < N / 2; ++i) { dist_[i] = real_max; dist_[i + N / 2] = -real_max; } } template KDOP::KDOP(const Vec3f& v) { for(size_t i = 0; i < 3; ++i) { dist_[i] = dist_[N / 2 + i] = v[i]; } FCL_REAL d[(N - 6) / 2]; getDistances<(N - 6) / 2>(v, d); for(size_t i = 0; i < (N - 6) / 2; ++i) { dist_[3 + i] = dist_[3 + i + N / 2] = d[i]; } } template KDOP::KDOP(const Vec3f& a, const Vec3f& b) { for(size_t i = 0; i < 3; ++i) { minmax(a[i], b[i], dist_[i], dist_[i + N / 2]); } FCL_REAL ad[(N - 6) / 2], bd[(N - 6) / 2]; getDistances<(N - 6) / 2>(a, ad); getDistances<(N - 6) / 2>(b, bd); for(size_t i = 0; i < (N - 6) / 2; ++i) { minmax(ad[i], bd[i], dist_[3 + i], dist_[3 + i + N / 2]); } } template bool KDOP::overlap(const KDOP& other) const { for(size_t i = 0; i < N / 2; ++i) { if(dist_[i] > other.dist_[i + N / 2]) return false; if(dist_[i + N / 2] < other.dist_[i]) return false; } return true; } template bool KDOP::inside(const Vec3f& p) const { for(size_t i = 0; i < 3; ++i) { if(p[i] < dist_[i] || p[i] > dist_[i + N / 2]) return false; } FCL_REAL d[(N - 6) / 2]; getDistances<(N - 6) / 2>(p, d); for(size_t i = 0; i < (N - 6) / 2; ++i) { if(d[i] < dist_[3 + i] || d[i] > dist_[i + 3 + N / 2]) return false; } return true; } template KDOP& KDOP::operator += (const Vec3f& p) { for(size_t i = 0; i < 3; ++i) { minmax(p[i], dist_[i], dist_[N / 2 + i]); } FCL_REAL pd[(N - 6) / 2]; getDistances<(N - 6) / 2>(p, pd); for(size_t i = 0; i < (N - 6) / 2; ++i) { minmax(pd[i], dist_[3 + i], dist_[3 + N / 2 + i]); } return *this; } template KDOP& KDOP::operator += (const KDOP& other) { for(size_t i = 0; i < N / 2; ++i) { dist_[i] = std::min(other.dist_[i], dist_[i]); dist_[i + N / 2] = std::max(other.dist_[i + N / 2], dist_[i + N / 2]); } return *this; } template KDOP KDOP::operator + (const KDOP& other) const { KDOP res(*this); return res += other; } template FCL_REAL KDOP::distance(const KDOP& other, Vec3f* P, Vec3f* Q) const { std::cerr << "KDOP distance not implemented!" << std::endl; return 0.0; } template KDOP translate(const KDOP& bv, const Vec3f& t) { KDOP res(bv); for(size_t i = 0; i < 3; ++i) { res.dist(i) += t[i]; res.dist(N / 2 + i) += t[i]; } FCL_REAL d[(N - 6) / 2]; getDistances<(N - 6) / 2>(t, d); for(size_t i = 0; i < (N - 6) / 2; ++i) { res.dist(3 + i) += d[i]; res.dist(3 + i + N / 2) += d[i]; } return res; } template class KDOP<16>; template class KDOP<18>; template class KDOP<24>; template KDOP<16> translate<16>(const KDOP<16>&, const Vec3f&); template KDOP<18> translate<18>(const KDOP<18>&, const Vec3f&); template KDOP<24> translate<24>(const KDOP<24>&, const Vec3f&); } fcl-0.5.0/src/BV/kIOS.cpp000066400000000000000000000121311274356571000146770ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/BV/kIOS.h" #include "fcl/BVH/BVH_utility.h" #include "fcl/math/transform.h" #include #include namespace fcl { bool kIOS::overlap(const kIOS& other) const { for(unsigned int i = 0; i < num_spheres; ++i) { for(unsigned int j = 0; j < other.num_spheres; ++j) { FCL_REAL o_dist = (spheres[i].o - other.spheres[j].o).sqrLength(); FCL_REAL sum_r = spheres[i].r + other.spheres[j].r; if(o_dist > sum_r * sum_r) return false; } } return obb.overlap(other.obb); return true; } bool kIOS::contain(const Vec3f& p) const { for(unsigned int i = 0; i < num_spheres; ++i) { FCL_REAL r = spheres[i].r; if((spheres[i].o - p).sqrLength() > r * r) return false; } return true; } kIOS& kIOS::operator += (const Vec3f& p) { for(unsigned int i = 0; i < num_spheres; ++i) { FCL_REAL r = spheres[i].r; FCL_REAL new_r_sqr = (p - spheres[i].o).sqrLength(); if(new_r_sqr > r * r) { spheres[i].r = sqrt(new_r_sqr); } } obb += p; return *this; } kIOS kIOS::operator + (const kIOS& other) const { kIOS result; unsigned int new_num_spheres = std::min(num_spheres, other.num_spheres); for(unsigned int i = 0; i < new_num_spheres; ++i) { result.spheres[i] = encloseSphere(spheres[i], other.spheres[i]); } result.num_spheres = new_num_spheres; result.obb = obb + other.obb; return result; } FCL_REAL kIOS::width() const { return obb.width(); } FCL_REAL kIOS::height() const { return obb.height(); } FCL_REAL kIOS::depth() const { return obb.depth(); } FCL_REAL kIOS::volume() const { return obb.volume(); } FCL_REAL kIOS::size() const { return volume(); } FCL_REAL kIOS::distance(const kIOS& other, Vec3f* P, Vec3f* Q) const { FCL_REAL d_max = 0; int id_a = -1, id_b = -1; for(unsigned int i = 0; i < num_spheres; ++i) { for(unsigned int j = 0; j < other.num_spheres; ++j) { FCL_REAL d = (spheres[i].o - other.spheres[j].o).length() - (spheres[i].r + other.spheres[j].r); if(d_max < d) { d_max = d; if(P && Q) { id_a = i; id_b = j; } } } } if(P && Q) { if(id_a != -1 && id_b != -1) { Vec3f v = spheres[id_a].o - spheres[id_b].o; FCL_REAL len_v = v.length(); *P = spheres[id_a].o - v * (spheres[id_a].r / len_v); *Q = spheres[id_b].o + v * (spheres[id_b].r / len_v); } } return d_max; } bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2) { kIOS b2_temp = b2; for(unsigned int i = 0; i < b2_temp.num_spheres; ++i) { b2_temp.spheres[i].o = R0 * b2_temp.spheres[i].o + T0; } b2_temp.obb.To = R0 * b2_temp.obb.To + T0; b2_temp.obb.axis[0] = R0 * b2_temp.obb.axis[0]; b2_temp.obb.axis[1] = R0 * b2_temp.obb.axis[1]; b2_temp.obb.axis[2] = R0 * b2_temp.obb.axis[2]; return b1.overlap(b2_temp); } FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2, Vec3f* P, Vec3f* Q) { kIOS b2_temp = b2; for(unsigned int i = 0; i < b2_temp.num_spheres; ++i) { b2_temp.spheres[i].o = R0 * b2_temp.spheres[i].o + T0; } return b1.distance(b2_temp, P, Q); } kIOS translate(const kIOS& bv, const Vec3f& t) { kIOS res(bv); for(size_t i = 0; i < res.num_spheres; ++i) { res.spheres[i].o += t; } translate(res.obb, t); return res; } } fcl-0.5.0/src/BVH/000077500000000000000000000000001274356571000135005ustar00rootroot00000000000000fcl-0.5.0/src/BVH/BVH_model.cpp000066400000000000000000000661671274356571000160230ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/BVH/BVH_model.h" #include "fcl/BV/BV.h" #include #include namespace fcl { template BVHModel::BVHModel(const BVHModel& other) : CollisionGeometry(other), num_tris(other.num_tris), num_vertices(other.num_vertices), build_state(other.build_state), bv_splitter(other.bv_splitter), bv_fitter(other.bv_fitter), num_tris_allocated(other.num_tris), num_vertices_allocated(other.num_vertices) { if(other.vertices) { vertices = new Vec3f[num_vertices]; memcpy(vertices, other.vertices, sizeof(Vec3f) * num_vertices); } else vertices = NULL; if(other.tri_indices) { tri_indices = new Triangle[num_tris]; memcpy(tri_indices, other.tri_indices, sizeof(Triangle) * num_tris); } else tri_indices = NULL; if(other.prev_vertices) { prev_vertices = new Vec3f[num_vertices]; memcpy(prev_vertices, other.prev_vertices, sizeof(Vec3f) * num_vertices); } else prev_vertices = NULL; if(other.primitive_indices) { int num_primitives = 0; switch(other.getModelType()) { case BVH_MODEL_TRIANGLES: num_primitives = num_tris; break; case BVH_MODEL_POINTCLOUD: num_primitives = num_vertices; break; default: ; } primitive_indices = new unsigned int[num_primitives]; memcpy(primitive_indices, other.primitive_indices, sizeof(unsigned int) * num_primitives); } else primitive_indices = NULL; num_bvs = num_bvs_allocated = other.num_bvs; if(other.bvs) { bvs = new BVNode[num_bvs]; memcpy(bvs, other.bvs, sizeof(BVNode) * num_bvs); } else bvs = NULL; } template int BVHModel::beginModel(int num_tris_, int num_vertices_) { if(build_state != BVH_BUILD_STATE_EMPTY) { delete [] vertices; vertices = NULL; delete [] tri_indices; tri_indices = NULL; delete [] bvs; bvs = NULL; delete [] prev_vertices; prev_vertices = NULL; delete [] primitive_indices; primitive_indices = NULL; num_vertices_allocated = num_vertices = num_tris_allocated = num_tris = num_bvs_allocated = num_bvs = 0; } if(num_tris_ <= 0) num_tris_ = 8; if(num_vertices_ <= 0) num_vertices_ = 8; num_vertices_allocated = num_vertices_; num_tris_allocated = num_tris_; tri_indices = new Triangle[num_tris_allocated]; vertices = new Vec3f[num_vertices_allocated]; if(!tri_indices) { std::cerr << "BVH Error! Out of memory for tri_indices array on BeginModel() call!" << std::endl; return BVH_ERR_MODEL_OUT_OF_MEMORY; } if(!vertices) { std::cerr << "BVH Error! Out of memory for vertices array on BeginModel() call!" << std::endl; return BVH_ERR_MODEL_OUT_OF_MEMORY; } if(build_state != BVH_BUILD_STATE_EMPTY) { std::cerr << "BVH Warning! Call beginModel() on a BVHModel that is not empty. This model was cleared and previous triangles/vertices were lost." << std::endl; build_state = BVH_BUILD_STATE_EMPTY; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } build_state = BVH_BUILD_STATE_BEGUN; return BVH_OK; } template int BVHModel::addVertex(const Vec3f& p) { if(build_state != BVH_BUILD_STATE_BEGUN) { std::cerr << "BVH Warning! Call addVertex() in a wrong order. addVertex() was ignored. Must do a beginModel() to clear the model for addition of new vertices." << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } if(num_vertices >= num_vertices_allocated) { Vec3f* temp = new Vec3f[num_vertices_allocated * 2]; if(!temp) { std::cerr << "BVH Error! Out of memory for vertices array on addVertex() call!" << std::endl; return BVH_ERR_MODEL_OUT_OF_MEMORY; } memcpy(temp, vertices, sizeof(Vec3f) * num_vertices); delete [] vertices; vertices = temp; num_vertices_allocated *= 2; } vertices[num_vertices] = p; num_vertices += 1; return BVH_OK; } template int BVHModel::addTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3) { if(build_state == BVH_BUILD_STATE_PROCESSED) { std::cerr << "BVH Warning! Call addTriangle() in a wrong order. addTriangle() was ignored. Must do a beginModel() to clear the model for addition of new triangles." << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } if(num_vertices + 2 >= num_vertices_allocated) { Vec3f* temp = new Vec3f[num_vertices_allocated * 2 + 2]; if(!temp) { std::cerr << "BVH Error! Out of memory for vertices array on addTriangle() call!" << std::endl; return BVH_ERR_MODEL_OUT_OF_MEMORY; } memcpy(temp, vertices, sizeof(Vec3f) * num_vertices); delete [] vertices; vertices = temp; num_vertices_allocated = num_vertices_allocated * 2 + 2; } int offset = num_vertices; vertices[num_vertices] = p1; num_vertices++; vertices[num_vertices] = p2; num_vertices++; vertices[num_vertices] = p3; num_vertices++; if(num_tris >= num_tris_allocated) { Triangle* temp = new Triangle[num_tris_allocated * 2]; if(!temp) { std::cerr << "BVH Error! Out of memory for tri_indices array on addTriangle() call!" << std::endl; return BVH_ERR_MODEL_OUT_OF_MEMORY; } memcpy(temp, tri_indices, sizeof(Triangle) * num_tris); delete [] tri_indices; tri_indices = temp; num_tris_allocated *= 2; } tri_indices[num_tris].set(offset, offset + 1, offset + 2); num_tris++; return BVH_OK; } template int BVHModel::addSubModel(const std::vector& ps) { if(build_state == BVH_BUILD_STATE_PROCESSED) { std::cerr << "BVH Warning! Call addSubModel() in a wrong order. addSubModel() was ignored. Must do a beginModel() to clear the model for addition of new vertices." << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } int num_vertices_to_add = ps.size(); if(num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated) { Vec3f* temp = new Vec3f[num_vertices_allocated * 2 + num_vertices_to_add - 1]; if(!temp) { std::cerr << "BVH Error! Out of memory for vertices array on addSubModel() call!" << std::endl; return BVH_ERR_MODEL_OUT_OF_MEMORY; } memcpy(temp, vertices, sizeof(Vec3f) * num_vertices); delete [] vertices; vertices = temp; num_vertices_allocated = num_vertices_allocated * 2 + num_vertices_to_add - 1; } for(int i = 0; i < num_vertices_to_add; ++i) { vertices[num_vertices] = ps[i]; num_vertices++; } return BVH_OK; } template int BVHModel::addSubModel(const std::vector& ps, const std::vector& ts) { if(build_state == BVH_BUILD_STATE_PROCESSED) { std::cerr << "BVH Warning! Call addSubModel() in a wrong order. addSubModel() was ignored. Must do a beginModel() to clear the model for addition of new vertices." << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } int num_vertices_to_add = ps.size(); if(num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated) { Vec3f* temp = new Vec3f[num_vertices_allocated * 2 + num_vertices_to_add - 1]; if(!temp) { std::cerr << "BVH Error! Out of memory for vertices array on addSubModel() call!" << std::endl; return BVH_ERR_MODEL_OUT_OF_MEMORY; } memcpy(temp, vertices, sizeof(Vec3f) * num_vertices); delete [] vertices; vertices = temp; num_vertices_allocated = num_vertices_allocated * 2 + num_vertices_to_add - 1; } int offset = num_vertices; for(int i = 0; i < num_vertices_to_add; ++i) { vertices[num_vertices] = ps[i]; num_vertices++; } int num_tris_to_add = ts.size(); if(num_tris + num_tris_to_add - 1 >= num_tris_allocated) { Triangle* temp = new Triangle[num_tris_allocated * 2 + num_tris_to_add - 1]; if(!temp) { std::cerr << "BVH Error! Out of memory for tri_indices array on addSubModel() call!" << std::endl; return BVH_ERR_MODEL_OUT_OF_MEMORY; } memcpy(temp, tri_indices, sizeof(Triangle) * num_tris); delete [] tri_indices; tri_indices = temp; num_tris_allocated = num_tris_allocated * 2 + num_tris_to_add - 1; } for(int i = 0; i < num_tris_to_add; ++i) { const Triangle& t = ts[i]; tri_indices[num_tris].set(t[0] + offset, t[1] + offset, t[2] + offset); num_tris++; } return BVH_OK; } template int BVHModel::endModel() { if(build_state != BVH_BUILD_STATE_BEGUN) { std::cerr << "BVH Warning! Call endModel() in wrong order. endModel() was ignored." << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } if(num_tris == 0 && num_vertices == 0) { std::cerr << "BVH Error! endModel() called on model with no triangles and vertices." << std::endl; return BVH_ERR_BUILD_EMPTY_MODEL; } if(num_tris_allocated > num_tris) { Triangle* new_tris = new Triangle[num_tris]; if(!new_tris) { std::cerr << "BVH Error! Out of memory for tri_indices array in endModel() call!" << std::endl; return BVH_ERR_MODEL_OUT_OF_MEMORY; } memcpy(new_tris, tri_indices, sizeof(Triangle) * num_tris); delete [] tri_indices; tri_indices = new_tris; num_tris_allocated = num_tris; } if(num_vertices_allocated > num_vertices) { Vec3f* new_vertices = new Vec3f[num_vertices]; if(!new_vertices) { std::cerr << "BVH Error! Out of memory for vertices array in endModel() call!" << std::endl; return BVH_ERR_MODEL_OUT_OF_MEMORY; } memcpy(new_vertices, vertices, sizeof(Vec3f) * num_vertices); delete [] vertices; vertices = new_vertices; num_vertices_allocated = num_vertices; } // construct BVH tree int num_bvs_to_be_allocated = 0; if(num_tris == 0) num_bvs_to_be_allocated = 2 * num_vertices - 1; else num_bvs_to_be_allocated = 2 * num_tris - 1; bvs = new BVNode [num_bvs_to_be_allocated]; primitive_indices = new unsigned int [num_bvs_to_be_allocated]; if(!bvs || !primitive_indices) { std::cerr << "BVH Error! Out of memory for BV array in endModel()!" << std::endl; return BVH_ERR_MODEL_OUT_OF_MEMORY; } num_bvs_allocated = num_bvs_to_be_allocated; num_bvs = 0; buildTree(); // finish constructing build_state = BVH_BUILD_STATE_PROCESSED; return BVH_OK; } template int BVHModel::beginReplaceModel() { if(build_state != BVH_BUILD_STATE_PROCESSED) { std::cerr << "BVH Error! Call beginReplaceModel() on a BVHModel that has no previous frame." << std::endl; return BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME; } if(prev_vertices) delete [] prev_vertices; prev_vertices = NULL; num_vertex_updated = 0; build_state = BVH_BUILD_STATE_REPLACE_BEGUN; return BVH_OK; } template int BVHModel::replaceVertex(const Vec3f& p) { if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN) { std::cerr << "BVH Warning! Call replaceVertex() in a wrong order. replaceVertex() was ignored. Must do a beginReplaceModel() for initialization." << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } vertices[num_vertex_updated] = p; num_vertex_updated++; return BVH_OK; } template int BVHModel::replaceTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3) { if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN) { std::cerr << "BVH Warning! Call replaceTriangle() in a wrong order. replaceTriangle() was ignored. Must do a beginReplaceModel() for initialization." << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } vertices[num_vertex_updated] = p1; num_vertex_updated++; vertices[num_vertex_updated] = p2; num_vertex_updated++; vertices[num_vertex_updated] = p3; num_vertex_updated++; return BVH_OK; } template int BVHModel::replaceSubModel(const std::vector& ps) { if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN) { std::cerr << "BVH Warning! Call replaceSubModel() in a wrong order. replaceSubModel() was ignored. Must do a beginReplaceModel() for initialization." << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } for(unsigned int i = 0; i < ps.size(); ++i) { vertices[num_vertex_updated] = ps[i]; num_vertex_updated++; } return BVH_OK; } template int BVHModel::endReplaceModel(bool refit, bool bottomup) { if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN) { std::cerr << "BVH Warning! Call endReplaceModel() in a wrong order. endReplaceModel() was ignored. " << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } if(num_vertex_updated != num_vertices) { std::cerr << "BVH Error! The replaced model should have the same number of vertices as the old model." << std::endl; return BVH_ERR_INCORRECT_DATA; } if(refit) // refit, do not change BVH structure { refitTree(bottomup); } else // reconstruct bvh tree based on current frame data { buildTree(); } build_state = BVH_BUILD_STATE_PROCESSED; return BVH_OK; } template int BVHModel::beginUpdateModel() { if(build_state != BVH_BUILD_STATE_PROCESSED && build_state != BVH_BUILD_STATE_UPDATED) { std::cerr << "BVH Error! Call beginUpdatemodel() on a BVHModel that has no previous frame." << std::endl; return BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME; } if(prev_vertices) { Vec3f* temp = prev_vertices; prev_vertices = vertices; vertices = temp; } else { prev_vertices = vertices; vertices = new Vec3f[num_vertices]; } num_vertex_updated = 0; build_state = BVH_BUILD_STATE_UPDATE_BEGUN; return BVH_OK; } template int BVHModel::updateVertex(const Vec3f& p) { if(build_state != BVH_BUILD_STATE_UPDATE_BEGUN) { std::cerr << "BVH Warning! Call updateVertex() in a wrong order. updateVertex() was ignored. Must do a beginUpdateModel() for initialization." << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } vertices[num_vertex_updated] = p; num_vertex_updated++; return BVH_OK; } template int BVHModel::updateTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3) { if(build_state != BVH_BUILD_STATE_UPDATE_BEGUN) { std::cerr << "BVH Warning! Call updateTriangle() in a wrong order. updateTriangle() was ignored. Must do a beginUpdateModel() for initialization." << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } vertices[num_vertex_updated] = p1; num_vertex_updated++; vertices[num_vertex_updated] = p2; num_vertex_updated++; vertices[num_vertex_updated] = p3; num_vertex_updated++; return BVH_OK; } template int BVHModel::updateSubModel(const std::vector& ps) { if(build_state != BVH_BUILD_STATE_UPDATE_BEGUN) { std::cerr << "BVH Warning! Call updateSubModel() in a wrong order. updateSubModel() was ignored. Must do a beginUpdateModel() for initialization." << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } for(unsigned int i = 0; i < ps.size(); ++i) { vertices[num_vertex_updated] = ps[i]; num_vertex_updated++; } return BVH_OK; } template int BVHModel::endUpdateModel(bool refit, bool bottomup) { if(build_state != BVH_BUILD_STATE_UPDATE_BEGUN) { std::cerr << "BVH Warning! Call endUpdateModel() in a wrong order. endUpdateModel() was ignored. " << std::endl; return BVH_ERR_BUILD_OUT_OF_SEQUENCE; } if(num_vertex_updated != num_vertices) { std::cerr << "BVH Error! The updated model should have the same number of vertices as the old model." << std::endl; return BVH_ERR_INCORRECT_DATA; } if(refit) // refit, do not change BVH structure { refitTree(bottomup); } else // reconstruct bvh tree based on current frame data { buildTree(); // then refit refitTree(bottomup); } build_state = BVH_BUILD_STATE_UPDATED; return BVH_OK; } template int BVHModel::memUsage(int msg) const { int mem_bv_list = sizeof(BV) * num_bvs; int mem_tri_list = sizeof(Triangle) * num_tris; int mem_vertex_list = sizeof(Vec3f) * num_vertices; int total_mem = mem_bv_list + mem_tri_list + mem_vertex_list + sizeof(BVHModel); if(msg) { std::cerr << "Total for model " << total_mem << " bytes." << std::endl; std::cerr << "BVs: " << num_bvs << " allocated." << std::endl; std::cerr << "Tris: " << num_tris << " allocated." << std::endl; std::cerr << "Vertices: " << num_vertices << " allocated." << std::endl; } return BVH_OK; } template int BVHModel::buildTree() { // set BVFitter bv_fitter->set(vertices, tri_indices, getModelType()); // set SplitRule bv_splitter->set(vertices, tri_indices, getModelType()); num_bvs = 1; int num_primitives = 0; switch(getModelType()) { case BVH_MODEL_TRIANGLES: num_primitives = num_tris; break; case BVH_MODEL_POINTCLOUD: num_primitives = num_vertices; break; default: std::cerr << "BVH Error: Model type not supported!" << std::endl; return BVH_ERR_UNSUPPORTED_FUNCTION; } for(int i = 0; i < num_primitives; ++i) primitive_indices[i] = i; recursiveBuildTree(0, 0, num_primitives); bv_fitter->clear(); bv_splitter->clear(); return BVH_OK; } template int BVHModel::recursiveBuildTree(int bv_id, int first_primitive, int num_primitives) { BVHModelType type = getModelType(); BVNode* bvnode = bvs + bv_id; unsigned int* cur_primitive_indices = primitive_indices + first_primitive; // constructing BV BV bv = bv_fitter->fit(cur_primitive_indices, num_primitives); bv_splitter->computeRule(bv, cur_primitive_indices, num_primitives); bvnode->bv = bv; bvnode->first_primitive = first_primitive; bvnode->num_primitives = num_primitives; if(num_primitives == 1) { bvnode->first_child = -((*cur_primitive_indices) + 1); } else { bvnode->first_child = num_bvs; num_bvs += 2; int c1 = 0; for(int i = 0; i < num_primitives; ++i) { Vec3f p; if(type == BVH_MODEL_POINTCLOUD) p = vertices[cur_primitive_indices[i]]; else if(type == BVH_MODEL_TRIANGLES) { const Triangle& t = tri_indices[cur_primitive_indices[i]]; const Vec3f& p1 = vertices[t[0]]; const Vec3f& p2 = vertices[t[1]]; const Vec3f& p3 = vertices[t[2]]; FCL_REAL x = (p1[0] + p2[0] + p3[0]) / 3.0; FCL_REAL y = (p1[1] + p2[1] + p3[1]) / 3.0; FCL_REAL z = (p1[2] + p2[2] + p3[2]) / 3.0; p.setValue(x, y, z); } else { std::cerr << "BVH Error: Model type not supported!" << std::endl; return BVH_ERR_UNSUPPORTED_FUNCTION; } // loop invariant: up to (but not including) index c1 in group 1, // then up to (but not including) index i in group 2 // // [1] [1] [1] [1] [2] [2] [2] [x] [x] ... [x] // c1 i // if(bv_splitter->apply(p)) // in the right side { // do nothing } else { unsigned int temp = cur_primitive_indices[i]; cur_primitive_indices[i] = cur_primitive_indices[c1]; cur_primitive_indices[c1] = temp; c1++; } } if((c1 == 0) || (c1 == num_primitives)) c1 = num_primitives / 2; int num_first_half = c1; recursiveBuildTree(bvnode->leftChild(), first_primitive, num_first_half); recursiveBuildTree(bvnode->rightChild(), first_primitive + num_first_half, num_primitives - num_first_half); } return BVH_OK; } template int BVHModel::refitTree(bool bottomup) { if(bottomup) return refitTree_bottomup(); else return refitTree_topdown(); } template int BVHModel::refitTree_bottomup() { int res = recursiveRefitTree_bottomup(0); return res; } template int BVHModel::recursiveRefitTree_bottomup(int bv_id) { BVNode* bvnode = bvs + bv_id; if(bvnode->isLeaf()) { BVHModelType type = getModelType(); int primitive_id = -(bvnode->first_child + 1); if(type == BVH_MODEL_POINTCLOUD) { BV bv; if(prev_vertices) { Vec3f v[2]; v[0] = prev_vertices[primitive_id]; v[1] = vertices[primitive_id]; fit(v, 2, bv); } else fit(vertices + primitive_id, 1, bv); bvnode->bv = bv; } else if(type == BVH_MODEL_TRIANGLES) { BV bv; const Triangle& triangle = tri_indices[primitive_id]; if(prev_vertices) { Vec3f v[6]; for(int i = 0; i < 3; ++i) { v[i] = prev_vertices[triangle[i]]; v[i + 3] = vertices[triangle[i]]; } fit(v, 6, bv); } else { Vec3f v[3]; for(int i = 0; i < 3; ++i) { v[i] = vertices[triangle[i]]; } fit(v, 3, bv); } bvnode->bv = bv; } else { std::cerr << "BVH Error: Model type not supported!" << std::endl; return BVH_ERR_UNSUPPORTED_FUNCTION; } } else { recursiveRefitTree_bottomup(bvnode->leftChild()); recursiveRefitTree_bottomup(bvnode->rightChild()); bvnode->bv = bvs[bvnode->leftChild()].bv + bvs[bvnode->rightChild()].bv; } return BVH_OK; } template int BVHModel::refitTree_topdown() { bv_fitter->set(vertices, prev_vertices, tri_indices, getModelType()); for(int i = 0; i < num_bvs; ++i) { BV bv = bv_fitter->fit(primitive_indices + bvs[i].first_primitive, bvs[i].num_primitives); bvs[i].bv = bv; } bv_fitter->clear(); return BVH_OK; } template void BVHModel::computeLocalAABB() { AABB aabb_; for(int i = 0; i < num_vertices; ++i) { aabb_ += vertices[i]; } aabb_center = aabb_.center(); aabb_radius = 0; for(int i = 0; i < num_vertices; ++i) { FCL_REAL r = (aabb_center - vertices[i]).sqrLength(); if(r > aabb_radius) aabb_radius = r; } aabb_radius = sqrt(aabb_radius); aabb_local = aabb_; } template<> void BVHModel::makeParentRelativeRecurse(int bv_id, Vec3f parent_axis[], const Vec3f& parent_c) { OBB& obb = bvs[bv_id].bv; if(!bvs[bv_id].isLeaf()) { makeParentRelativeRecurse(bvs[bv_id].first_child, obb.axis, obb.To); makeParentRelativeRecurse(bvs[bv_id].first_child + 1, obb.axis, obb.To); } // make self parent relative obb.axis[0] = Vec3f(parent_axis[0].dot(obb.axis[0]), parent_axis[1].dot(obb.axis[0]), parent_axis[2].dot(obb.axis[0])); obb.axis[1] = Vec3f(parent_axis[0].dot(obb.axis[1]), parent_axis[1].dot(obb.axis[1]), parent_axis[2].dot(obb.axis[1])); obb.axis[2] = Vec3f(parent_axis[0].dot(obb.axis[2]), parent_axis[1].dot(obb.axis[2]), parent_axis[2].dot(obb.axis[2])); Vec3f t = obb.To - parent_c; obb.To = Vec3f(parent_axis[0].dot(t), parent_axis[1].dot(t), parent_axis[2].dot(t)); } template<> void BVHModel::makeParentRelativeRecurse(int bv_id, Vec3f parent_axis[], const Vec3f& parent_c) { RSS& rss = bvs[bv_id].bv; if(!bvs[bv_id].isLeaf()) { makeParentRelativeRecurse(bvs[bv_id].first_child, rss.axis, rss.Tr); makeParentRelativeRecurse(bvs[bv_id].first_child + 1, rss.axis, rss.Tr); } // make self parent relative rss.axis[0] = Vec3f(parent_axis[0].dot(rss.axis[0]), parent_axis[1].dot(rss.axis[0]), parent_axis[2].dot(rss.axis[0])); rss.axis[1] = Vec3f(parent_axis[0].dot(rss.axis[1]), parent_axis[1].dot(rss.axis[1]), parent_axis[2].dot(rss.axis[1])); rss.axis[2] = Vec3f(parent_axis[0].dot(rss.axis[2]), parent_axis[1].dot(rss.axis[2]), parent_axis[2].dot(rss.axis[2])); Vec3f t = rss.Tr - parent_c; rss.Tr = Vec3f(parent_axis[0].dot(t), parent_axis[1].dot(t), parent_axis[2].dot(t)); } template<> void BVHModel::makeParentRelativeRecurse(int bv_id, Vec3f parent_axis[], const Vec3f& parent_c) { OBB& obb = bvs[bv_id].bv.obb; RSS& rss = bvs[bv_id].bv.rss; if(!bvs[bv_id].isLeaf()) { makeParentRelativeRecurse(bvs[bv_id].first_child, obb.axis, obb.To); makeParentRelativeRecurse(bvs[bv_id].first_child + 1, obb.axis, obb.To); } // make self parent relative obb.axis[0] = Vec3f(parent_axis[0].dot(obb.axis[0]), parent_axis[1].dot(obb.axis[0]), parent_axis[2].dot(obb.axis[0])); obb.axis[1] = Vec3f(parent_axis[0].dot(obb.axis[1]), parent_axis[1].dot(obb.axis[1]), parent_axis[2].dot(obb.axis[1])); obb.axis[2] = Vec3f(parent_axis[0].dot(obb.axis[2]), parent_axis[1].dot(obb.axis[2]), parent_axis[2].dot(obb.axis[2])); rss.axis[0] = obb.axis[0]; rss.axis[1] = obb.axis[1]; rss.axis[2] = obb.axis[2]; Vec3f t = obb.To - parent_c; obb.To = Vec3f(parent_axis[0].dot(t), parent_axis[1].dot(t), parent_axis[2].dot(t)); rss.Tr = obb.To; } template<> NODE_TYPE BVHModel::getNodeType() const { return BV_AABB; } template<> NODE_TYPE BVHModel::getNodeType() const { return BV_OBB; } template<> NODE_TYPE BVHModel::getNodeType() const { return BV_RSS; } template<> NODE_TYPE BVHModel::getNodeType() const { return BV_kIOS; } template<> NODE_TYPE BVHModel::getNodeType() const { return BV_OBBRSS; } template<> NODE_TYPE BVHModel >::getNodeType() const { return BV_KDOP16; } template<> NODE_TYPE BVHModel >::getNodeType() const { return BV_KDOP18; } template<> NODE_TYPE BVHModel >::getNodeType() const { return BV_KDOP24; } template class BVHModel >; template class BVHModel >; template class BVHModel >; template class BVHModel; template class BVHModel; template class BVHModel; template class BVHModel; template class BVHModel; } fcl-0.5.0/src/BVH/BVH_utility.cpp000066400000000000000000000435411274356571000164150ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/BVH/BVH_utility.h" namespace fcl { void BVHExpand(BVHModel& model, const Variance3f* ucs, FCL_REAL r = 1.0) { for(int i = 0; i < model.getNumBVs(); ++i) { BVNode& bvnode = model.getBV(i); Vec3f* vs = new Vec3f[bvnode.num_primitives * 6]; for(int j = 0; j < bvnode.num_primitives; ++j) { int v_id = bvnode.first_primitive + j; const Variance3f& uc = ucs[v_id]; Vec3f&v = model.vertices[bvnode.first_primitive + j]; for(int k = 0; k < 3; ++k) { vs[6 * j + 2 * k] = v + uc.axis[k] * (r * uc.sigma[k]); vs[6 * j + 2 * k + 1] = v - uc.axis[k] * (r * uc.sigma[k]); } } OBB bv; fit(vs, bvnode.num_primitives * 6, bv); delete [] vs; bvnode.bv = bv; } } void BVHExpand(BVHModel& model, const Variance3f* ucs, FCL_REAL r = 1.0) { for(int i = 0; i < model.getNumBVs(); ++i) { BVNode& bvnode = model.getBV(i); Vec3f* vs = new Vec3f[bvnode.num_primitives * 6]; for(int j = 0; j < bvnode.num_primitives; ++j) { int v_id = bvnode.first_primitive + j; const Variance3f& uc = ucs[v_id]; Vec3f&v = model.vertices[bvnode.first_primitive + j]; for(int k = 0; k < 3; ++k) { vs[6 * j + 2 * k] = v + uc.axis[k] * (r * uc.sigma[k]); vs[6 * j + 2 * k + 1] = v - uc.axis[k] * (r * uc.sigma[k]); } } RSS bv; fit(vs, bvnode.num_primitives * 6, bv); delete [] vs; bvnode.bv = bv; } } void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Matrix3f& M) { Vec3f S1; Vec3f S2[3]; if(ts) { for(int i = 0; i < n; ++i) { const Triangle& t = (indices) ? ts[indices[i]] : ts[i]; const Vec3f& p1 = ps[t[0]]; const Vec3f& p2 = ps[t[1]]; const Vec3f& p3 = ps[t[2]]; S1[0] += (p1[0] + p2[0] + p3[0]); S1[1] += (p1[1] + p2[1] + p3[1]); S1[2] += (p1[2] + p2[2] + p3[2]); S2[0][0] += (p1[0] * p1[0] + p2[0] * p2[0] + p3[0] * p3[0]); S2[1][1] += (p1[1] * p1[1] + p2[1] * p2[1] + p3[1] * p3[1]); S2[2][2] += (p1[2] * p1[2] + p2[2] * p2[2] + p3[2] * p3[2]); S2[0][1] += (p1[0] * p1[1] + p2[0] * p2[1] + p3[0] * p3[1]); S2[0][2] += (p1[0] * p1[2] + p2[0] * p2[2] + p3[0] * p3[2]); S2[1][2] += (p1[1] * p1[2] + p2[1] * p2[2] + p3[1] * p3[2]); if(ps2) { const Vec3f& p1 = ps2[t[0]]; const Vec3f& p2 = ps2[t[1]]; const Vec3f& p3 = ps2[t[2]]; S1[0] += (p1[0] + p2[0] + p3[0]); S1[1] += (p1[1] + p2[1] + p3[1]); S1[2] += (p1[2] + p2[2] + p3[2]); S2[0][0] += (p1[0] * p1[0] + p2[0] * p2[0] + p3[0] * p3[0]); S2[1][1] += (p1[1] * p1[1] + p2[1] * p2[1] + p3[1] * p3[1]); S2[2][2] += (p1[2] * p1[2] + p2[2] * p2[2] + p3[2] * p3[2]); S2[0][1] += (p1[0] * p1[1] + p2[0] * p2[1] + p3[0] * p3[1]); S2[0][2] += (p1[0] * p1[2] + p2[0] * p2[2] + p3[0] * p3[2]); S2[1][2] += (p1[1] * p1[2] + p2[1] * p2[2] + p3[1] * p3[2]); } } } else { for(int i = 0; i < n; ++i) { const Vec3f& p = (indices) ? ps[indices[i]] : ps[i]; S1 += p; S2[0][0] += (p[0] * p[0]); S2[1][1] += (p[1] * p[1]); S2[2][2] += (p[2] * p[2]); S2[0][1] += (p[0] * p[1]); S2[0][2] += (p[0] * p[2]); S2[1][2] += (p[1] * p[2]); if(ps2) // another frame { const Vec3f& p = (indices) ? ps2[indices[i]] : ps2[i]; S1 += p; S2[0][0] += (p[0] * p[0]); S2[1][1] += (p[1] * p[1]); S2[2][2] += (p[2] * p[2]); S2[0][1] += (p[0] * p[1]); S2[0][2] += (p[0] * p[2]); S2[1][2] += (p[1] * p[2]); } } } int n_points = ((ps2) ? 2 : 1) * ((ts) ? 3 : 1) * n; M(0, 0) = S2[0][0] - S1[0]*S1[0] / n_points; M(1, 1) = S2[1][1] - S1[1]*S1[1] / n_points; M(2, 2) = S2[2][2] - S1[2]*S1[2] / n_points; M(0, 1) = S2[0][1] - S1[0]*S1[1] / n_points; M(1, 2) = S2[1][2] - S1[1]*S1[2] / n_points; M(0, 2) = S2[0][2] - S1[0]*S1[2] / n_points; M(1, 0) = M(0, 1); M(2, 0) = M(0, 2); M(2, 1) = M(1, 2); } /** \brief Compute the RSS bounding volume parameters: radius, rectangle size and the origin. * The bounding volume axes are known. */ void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f axis[3], Vec3f& origin, FCL_REAL l[2], FCL_REAL& r) { bool indirect_index = true; if(!indices) indirect_index = false; int size_P = ((ps2) ? 2 : 1) * ((ts) ? 3 : 1) * n; FCL_REAL (*P)[3] = new FCL_REAL[size_P][3]; int P_id = 0; if(ts) { for(int i = 0; i < n; ++i) { int index = indirect_index ? indices[i] : i; const Triangle& t = ts[index]; for(int j = 0; j < 3; ++j) { int point_id = t[j]; const Vec3f& p = ps[point_id]; Vec3f v(p[0], p[1], p[2]); P[P_id][0] = axis[0].dot(v); P[P_id][1] = axis[1].dot(v); P[P_id][2] = axis[2].dot(v); P_id++; } if(ps2) { for(int j = 0; j < 3; ++j) { int point_id = t[j]; const Vec3f& p = ps2[point_id]; Vec3f v(p[0], p[1], p[2]); P[P_id][0] = axis[0].dot(v); P[P_id][1] = axis[0].dot(v); P[P_id][2] = axis[1].dot(v); P_id++; } } } } else { for(int i = 0; i < n; ++i) { int index = indirect_index ? indices[i] : i; const Vec3f& p = ps[index]; Vec3f v(p[0], p[1], p[2]); P[P_id][0] = axis[0].dot(v); P[P_id][1] = axis[1].dot(v); P[P_id][2] = axis[2].dot(v); P_id++; if(ps2) { const Vec3f& v = ps2[index]; P[P_id][0] = axis[0].dot(v); P[P_id][1] = axis[1].dot(v); P[P_id][2] = axis[2].dot(v); P_id++; } } } FCL_REAL minx, maxx, miny, maxy, minz, maxz; FCL_REAL cz, radsqr; minz = maxz = P[0][2]; for(int i = 1; i < size_P; ++i) { FCL_REAL z_value = P[i][2]; if(z_value < minz) minz = z_value; else if(z_value > maxz) maxz = z_value; } r = (FCL_REAL)0.5 * (maxz - minz); radsqr = r * r; cz = (FCL_REAL)0.5 * (maxz + minz); // compute an initial length of rectangle along x direction // find minx and maxx as starting points int minindex, maxindex; minindex = maxindex = 0; FCL_REAL mintmp, maxtmp; mintmp = maxtmp = P[0][0]; for(int i = 1; i < size_P; ++i) { FCL_REAL x_value = P[i][0]; if(x_value < mintmp) { minindex = i; mintmp = x_value; } else if(x_value > maxtmp) { maxindex = i; maxtmp = x_value; } } FCL_REAL x, dz; dz = P[minindex][2] - cz; minx = P[minindex][0] + std::sqrt(std::max(radsqr - dz * dz, 0)); dz = P[maxindex][2] - cz; maxx = P[maxindex][0] - std::sqrt(std::max(radsqr - dz * dz, 0)); // grow minx for(int i = 0; i < size_P; ++i) { if(P[i][0] < minx) { dz = P[i][2] - cz; x = P[i][0] + std::sqrt(std::max(radsqr - dz * dz, 0)); if(x < minx) minx = x; } } // grow maxx for(int i = 0; i < size_P; ++i) { if(P[i][0] > maxx) { dz = P[i][2] - cz; x = P[i][0] - std::sqrt(std::max(radsqr - dz * dz, 0)); if(x > maxx) maxx = x; } } // compute an initial length of rectangle along y direction // find miny and maxy as starting points minindex = maxindex = 0; mintmp = maxtmp = P[0][1]; for(int i = 1; i < size_P; ++i) { FCL_REAL y_value = P[i][1]; if(y_value < mintmp) { minindex = i; mintmp = y_value; } else if(y_value > maxtmp) { maxindex = i; maxtmp = y_value; } } FCL_REAL y; dz = P[minindex][2] - cz; miny = P[minindex][1] + std::sqrt(std::max(radsqr - dz * dz, 0)); dz = P[maxindex][2] - cz; maxy = P[maxindex][1] - std::sqrt(std::max(radsqr - dz * dz, 0)); // grow miny for(int i = 0; i < size_P; ++i) { if(P[i][1] < miny) { dz = P[i][2] - cz; y = P[i][1] + std::sqrt(std::max(radsqr - dz * dz, 0)); if(y < miny) miny = y; } } // grow maxy for(int i = 0; i < size_P; ++i) { if(P[i][1] > maxy) { dz = P[i][2] - cz; y = P[i][1] - std::sqrt(std::max(radsqr - dz * dz, 0)); if(y > maxy) maxy = y; } } // corners may have some points which are not covered - grow lengths if necessary // quite conservative (can be improved) FCL_REAL dx, dy, u, t; FCL_REAL a = std::sqrt((FCL_REAL)0.5); for(int i = 0; i < size_P; ++i) { if(P[i][0] > maxx) { if(P[i][1] > maxy) { dx = P[i][0] - maxx; dy = P[i][1] - maxy; u = dx * a + dy * a; t = (a*u - dx)*(a*u - dx) + (a*u - dy)*(a*u - dy) + (cz - P[i][2])*(cz - P[i][2]); u = u - std::sqrt(std::max(radsqr - t, 0)); if(u > 0) { maxx += u*a; maxy += u*a; } } else if(P[i][1] < miny) { dx = P[i][0] - maxx; dy = P[i][1] - miny; u = dx * a - dy * a; t = (a*u - dx)*(a*u - dx) + (-a*u - dy)*(-a*u - dy) + (cz - P[i][2])*(cz - P[i][2]); u = u - std::sqrt(std::max(radsqr - t, 0)); if(u > 0) { maxx += u*a; miny -= u*a; } } } else if(P[i][0] < minx) { if(P[i][1] > maxy) { dx = P[i][0] - minx; dy = P[i][1] - maxy; u = dy * a - dx * a; t = (-a*u - dx)*(-a*u - dx) + (a*u - dy)*(a*u - dy) + (cz - P[i][2])*(cz - P[i][2]); u = u - std::sqrt(std::max(radsqr - t, 0)); if(u > 0) { minx -= u*a; maxy += u*a; } } else if(P[i][1] < miny) { dx = P[i][0] - minx; dy = P[i][1] - miny; u = -dx * a - dy * a; t = (-a*u - dx)*(-a*u - dx) + (-a*u - dy)*(-a*u - dy) + (cz - P[i][2])*(cz - P[i][2]); u = u - std::sqrt(std::max(radsqr - t, 0)); if (u > 0) { minx -= u*a; miny -= u*a; } } } } origin = axis[0] * minx + axis[1] * miny + axis[2] * cz; l[0] = maxx - minx; if(l[0] < 0) l[0] = 0; l[1] = maxy - miny; if(l[1] < 0) l[1] = 0; delete [] P; } /** \brief Compute the bounding volume extent and center for a set or subset of points. * The bounding volume axes are known. */ static inline void getExtentAndCenter_pointcloud(Vec3f* ps, Vec3f* ps2, unsigned int* indices, int n, Vec3f axis[3], Vec3f& center, Vec3f& extent) { bool indirect_index = true; if(!indices) indirect_index = false; FCL_REAL real_max = std::numeric_limits::max(); FCL_REAL min_coord[3] = {real_max, real_max, real_max}; FCL_REAL max_coord[3] = {-real_max, -real_max, -real_max}; for(int i = 0; i < n; ++i) { int index = indirect_index ? indices[i] : i; const Vec3f& p = ps[index]; Vec3f v(p[0], p[1], p[2]); FCL_REAL proj[3]; proj[0] = axis[0].dot(v); proj[1] = axis[1].dot(v); proj[2] = axis[2].dot(v); for(int j = 0; j < 3; ++j) { if(proj[j] > max_coord[j]) max_coord[j] = proj[j]; if(proj[j] < min_coord[j]) min_coord[j] = proj[j]; } if(ps2) { const Vec3f& v = ps2[index]; proj[0] = axis[0].dot(v); proj[1] = axis[1].dot(v); proj[2] = axis[2].dot(v); for(int j = 0; j < 3; ++j) { if(proj[j] > max_coord[j]) max_coord[j] = proj[j]; if(proj[j] < min_coord[j]) min_coord[j] = proj[j]; } } } Vec3f o((max_coord[0] + min_coord[0]) / 2, (max_coord[1] + min_coord[1]) / 2, (max_coord[2] + min_coord[2]) / 2); center = axis[0] * o[0] + axis[1] * o[1] + axis[2] * o[2]; extent.setValue((max_coord[0] - min_coord[0]) / 2, (max_coord[1] - min_coord[1]) / 2, (max_coord[2] - min_coord[2]) / 2); } /** \brief Compute the bounding volume extent and center for a set or subset of points. * The bounding volume axes are known. */ static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f axis[3], Vec3f& center, Vec3f& extent) { bool indirect_index = true; if(!indices) indirect_index = false; FCL_REAL real_max = std::numeric_limits::max(); FCL_REAL min_coord[3] = {real_max, real_max, real_max}; FCL_REAL max_coord[3] = {-real_max, -real_max, -real_max}; for(int i = 0; i < n; ++i) { unsigned int index = indirect_index? indices[i] : i; const Triangle& t = ts[index]; for(int j = 0; j < 3; ++j) { int point_id = t[j]; const Vec3f& p = ps[point_id]; Vec3f v(p[0], p[1], p[2]); FCL_REAL proj[3]; proj[0] = axis[0].dot(v); proj[1] = axis[1].dot(v); proj[2] = axis[2].dot(v); for(int k = 0; k < 3; ++k) { if(proj[k] > max_coord[k]) max_coord[k] = proj[k]; if(proj[k] < min_coord[k]) min_coord[k] = proj[k]; } } if(ps2) { for(int j = 0; j < 3; ++j) { int point_id = t[j]; const Vec3f& p = ps2[point_id]; Vec3f v(p[0], p[1], p[2]); FCL_REAL proj[3]; proj[0] = axis[0].dot(v); proj[1] = axis[1].dot(v); proj[2] = axis[2].dot(v); for(int k = 0; k < 3; ++k) { if(proj[k] > max_coord[k]) max_coord[k] = proj[k]; if(proj[k] < min_coord[k]) min_coord[k] = proj[k]; } } } } Vec3f o((max_coord[0] + min_coord[0]) / 2, (max_coord[1] + min_coord[1]) / 2, (max_coord[2] + min_coord[2]) / 2); center = axis[0] * o[0] + axis[1] * o[1] + axis[2] * o[2]; extent.setValue((max_coord[0] - min_coord[0]) / 2, (max_coord[1] - min_coord[1]) / 2, (max_coord[2] - min_coord[2]) / 2); } void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f axis[3], Vec3f& center, Vec3f& extent) { if(ts) getExtentAndCenter_mesh(ps, ps2, ts, indices, n, axis, center, extent); else getExtentAndCenter_pointcloud(ps, ps2, indices, n, axis, center, extent); } void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c, Vec3f& center, FCL_REAL& radius) { Vec3f e1 = a - c; Vec3f e2 = b - c; FCL_REAL e1_len2 = e1.sqrLength(); FCL_REAL e2_len2 = e2.sqrLength(); Vec3f e3 = e1.cross(e2); FCL_REAL e3_len2 = e3.sqrLength(); radius = e1_len2 * e2_len2 * (e1 - e2).sqrLength() / e3_len2; radius = std::sqrt(radius) * 0.5; center = (e2 * e1_len2 - e1 * e2_len2).cross(e3) * (0.5 * 1 / e3_len2) + c; } static inline FCL_REAL maximumDistance_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, const Vec3f& query) { bool indirect_index = true; if(!indices) indirect_index = false; FCL_REAL maxD = 0; for(int i = 0; i < n; ++i) { unsigned int index = indirect_index ? indices[i] : i; const Triangle& t = ts[index]; for(int j = 0; j < 3; ++j) { int point_id = t[j]; const Vec3f& p = ps[point_id]; FCL_REAL d = (p - query).sqrLength(); if(d > maxD) maxD = d; } if(ps2) { for(int j = 0; j < 3; ++j) { int point_id = t[j]; const Vec3f& p = ps2[point_id]; FCL_REAL d = (p - query).sqrLength(); if(d > maxD) maxD = d; } } } return std::sqrt(maxD); } static inline FCL_REAL maximumDistance_pointcloud(Vec3f* ps, Vec3f* ps2, unsigned int* indices, int n, const Vec3f& query) { bool indirect_index = true; if(!indices) indirect_index = false; FCL_REAL maxD = 0; for(int i = 0; i < n; ++i) { int index = indirect_index ? indices[i] : i; const Vec3f& p = ps[index]; FCL_REAL d = (p - query).sqrLength(); if(d > maxD) maxD = d; if(ps2) { const Vec3f& v = ps2[index]; FCL_REAL d = (v - query).sqrLength(); if(d > maxD) maxD = d; } } return std::sqrt(maxD); } FCL_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, const Vec3f& query) { if(ts) return maximumDistance_mesh(ps, ps2, ts, indices, n, query); else return maximumDistance_pointcloud(ps, ps2, indices, n, query); } } fcl-0.5.0/src/BVH/BV_fitter.cpp000066400000000000000000000377351274356571000161070ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/BVH/BV_fitter.h" #include "fcl/BVH/BVH_utility.h" #include namespace fcl { static const double kIOS_RATIO = 1.5; static const double invSinA = 2; static const double invCosA = 2.0 / sqrt(3.0); // static const double sinA = 0.5; static const double cosA = sqrt(3.0) / 2.0; static inline void axisFromEigen(Vec3f eigenV[3], Matrix3f::U eigenS[3], Vec3f axis[3]) { int min, mid, max; if(eigenS[0] > eigenS[1]) { max = 0; min = 1; } else { min = 0; max = 1; } if(eigenS[2] < eigenS[min]) { mid = min; min = 2; } else if(eigenS[2] > eigenS[max]) { mid = max; max = 2; } else { mid = 2; } axis[0].setValue(eigenV[0][max], eigenV[1][max], eigenV[2][max]); axis[1].setValue(eigenV[0][mid], eigenV[1][mid], eigenV[2][mid]); axis[2].setValue(eigenV[1][max]*eigenV[2][mid] - eigenV[1][mid]*eigenV[2][max], eigenV[0][mid]*eigenV[2][max] - eigenV[0][max]*eigenV[2][mid], eigenV[0][max]*eigenV[1][mid] - eigenV[0][mid]*eigenV[1][max]); } namespace OBB_fit_functions { void fit1(Vec3f* ps, OBB& bv) { bv.To = ps[0]; bv.axis[0].setValue(1, 0, 0); bv.axis[1].setValue(0, 1, 0); bv.axis[2].setValue(0, 0, 1); bv.extent.setValue(0); } void fit2(Vec3f* ps, OBB& bv) { const Vec3f& p1 = ps[0]; const Vec3f& p2 = ps[1]; Vec3f p1p2 = p1 - p2; FCL_REAL len_p1p2 = p1p2.length(); p1p2.normalize(); bv.axis[0] = p1p2; generateCoordinateSystem(bv.axis[0], bv.axis[1], bv.axis[2]); bv.extent.setValue(len_p1p2 * 0.5, 0, 0); bv.To.setValue(0.5 * (p1[0] + p2[0]), 0.5 * (p1[1] + p2[1]), 0.5 * (p1[2] + p2[2])); } void fit3(Vec3f* ps, OBB& bv) { const Vec3f& p1 = ps[0]; const Vec3f& p2 = ps[1]; const Vec3f& p3 = ps[2]; Vec3f e[3]; e[0] = p1 - p2; e[1] = p2 - p3; e[2] = p3 - p1; FCL_REAL len[3]; len[0] = e[0].sqrLength(); len[1] = e[1].sqrLength(); len[2] = e[2].sqrLength(); int imax = 0; if(len[1] > len[0]) imax = 1; if(len[2] > len[imax]) imax = 2; Vec3f& u = bv.axis[0]; Vec3f& v = bv.axis[1]; Vec3f& w = bv.axis[2]; w = e[0].cross(e[1]); w.normalize(); u = e[imax]; u.normalize(); v = w.cross(u); getExtentAndCenter(ps, NULL, NULL, NULL, 3, bv.axis, bv.To, bv.extent); } void fit6(Vec3f* ps, OBB& bv) { OBB bv1, bv2; fit3(ps, bv1); fit3(ps + 3, bv2); bv = bv1 + bv2; } void fitn(Vec3f* ps, int n, OBB& bv) { Matrix3f M; Vec3f E[3]; Matrix3f::U s[3] = {0, 0, 0}; // three eigen values getCovariance(ps, NULL, NULL, NULL, n, M); eigen(M, s, E); axisFromEigen(E, s, bv.axis); // set obb centers and extensions getExtentAndCenter(ps, NULL, NULL, NULL, n, bv.axis, bv.To, bv.extent); } } namespace RSS_fit_functions { void fit1(Vec3f* ps, RSS& bv) { bv.Tr = ps[0]; bv.axis[0].setValue(1, 0, 0); bv.axis[1].setValue(0, 1, 0); bv.axis[2].setValue(0, 0, 1); bv.l[0] = 0; bv.l[1] = 0; bv.r = 0; } void fit2(Vec3f* ps, RSS& bv) { const Vec3f& p1 = ps[0]; const Vec3f& p2 = ps[1]; Vec3f p1p2 = p1 - p2; FCL_REAL len_p1p2 = p1p2.length(); p1p2.normalize(); bv.axis[0] = p1p2; generateCoordinateSystem(bv.axis[0], bv.axis[1], bv.axis[2]); bv.l[0] = len_p1p2; bv.l[1] = 0; bv.Tr = p2; bv.r = 0; } void fit3(Vec3f* ps, RSS& bv) { const Vec3f& p1 = ps[0]; const Vec3f& p2 = ps[1]; const Vec3f& p3 = ps[2]; Vec3f e[3]; e[0] = p1 - p2; e[1] = p2 - p3; e[2] = p3 - p1; FCL_REAL len[3]; len[0] = e[0].sqrLength(); len[1] = e[1].sqrLength(); len[2] = e[2].sqrLength(); int imax = 0; if(len[1] > len[0]) imax = 1; if(len[2] > len[imax]) imax = 2; Vec3f& u = bv.axis[0]; Vec3f& v = bv.axis[1]; Vec3f& w = bv.axis[2]; w = e[0].cross(e[1]); w.normalize(); u = e[imax]; u.normalize(); v = w.cross(u); getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, NULL, 3, bv.axis, bv.Tr, bv.l, bv.r); } void fit6(Vec3f* ps, RSS& bv) { RSS bv1, bv2; fit3(ps, bv1); fit3(ps + 3, bv2); bv = bv1 + bv2; } void fitn(Vec3f* ps, int n, RSS& bv) { Matrix3f M; // row first matrix Vec3f E[3]; // row first eigen-vectors Matrix3f::U s[3] = {0, 0, 0}; getCovariance(ps, NULL, NULL, NULL, n, M); eigen(M, s, E); axisFromEigen(E, s, bv.axis); // set rss origin, rectangle size and radius getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, NULL, n, bv.axis, bv.Tr, bv.l, bv.r); } } namespace kIOS_fit_functions { void fit1(Vec3f* ps, kIOS& bv) { bv.num_spheres = 1; bv.spheres[0].o = ps[0]; bv.spheres[0].r = 0; bv.obb.axis[0].setValue(1, 0, 0); bv.obb.axis[1].setValue(0, 1, 0); bv.obb.axis[2].setValue(0, 0, 1); bv.obb.extent.setValue(0); bv.obb.To = ps[0]; } void fit2(Vec3f* ps, kIOS& bv) { bv.num_spheres = 5; const Vec3f& p1 = ps[0]; const Vec3f& p2 = ps[1]; Vec3f p1p2 = p1 - p2; FCL_REAL len_p1p2 = p1p2.length(); p1p2.normalize(); Vec3f* axis = bv.obb.axis; axis[0] = p1p2; generateCoordinateSystem(axis[0], axis[1], axis[2]); FCL_REAL r0 = len_p1p2 * 0.5; bv.obb.extent.setValue(r0, 0, 0); bv.obb.To = (p1 + p2) * 0.5; bv.spheres[0].o = bv.obb.To; bv.spheres[0].r = r0; FCL_REAL r1 = r0 * invSinA; FCL_REAL r1cosA = r1 * cosA; bv.spheres[1].r = r1; bv.spheres[2].r = r1; Vec3f delta = axis[1] * r1cosA; bv.spheres[1].o = bv.spheres[0].o - delta; bv.spheres[2].o = bv.spheres[0].o + delta; bv.spheres[3].r = r1; bv.spheres[4].r = r1; delta = axis[2] * r1cosA; bv.spheres[3].o = bv.spheres[0].o - delta; bv.spheres[4].o = bv.spheres[0].o + delta; } void fit3(Vec3f* ps, kIOS& bv) { bv.num_spheres = 3; const Vec3f& p1 = ps[0]; const Vec3f& p2 = ps[1]; const Vec3f& p3 = ps[2]; Vec3f e[3]; e[0] = p1 - p2; e[1] = p2 - p3; e[2] = p3 - p1; FCL_REAL len[3]; len[0] = e[0].sqrLength(); len[1] = e[1].sqrLength(); len[2] = e[2].sqrLength(); int imax = 0; if(len[1] > len[0]) imax = 1; if(len[2] > len[imax]) imax = 2; Vec3f& u = bv.obb.axis[0]; Vec3f& v = bv.obb.axis[1]; Vec3f& w = bv.obb.axis[2]; w = e[0].cross(e[1]); w.normalize(); u = e[imax]; u.normalize(); v = w.cross(u); getExtentAndCenter(ps, NULL, NULL, NULL, 3, bv.obb.axis, bv.obb.To, bv.obb.extent); // compute radius and center FCL_REAL r0; Vec3f center; circumCircleComputation(p1, p2, p3, center, r0); bv.spheres[0].o = center; bv.spheres[0].r = r0; FCL_REAL r1 = r0 * invSinA; Vec3f delta = bv.obb.axis[2] * (r1 * cosA); bv.spheres[1].r = r1; bv.spheres[1].o = center - delta; bv.spheres[2].r = r1; bv.spheres[2].o = center + delta; } void fitn(Vec3f* ps, int n, kIOS& bv) { Matrix3f M; Vec3f E[3]; Matrix3f::U s[3] = {0, 0, 0}; // three eigen values; getCovariance(ps, NULL, NULL, NULL, n, M); eigen(M, s, E); Vec3f* axis = bv.obb.axis; axisFromEigen(E, s, axis); getExtentAndCenter(ps, NULL, NULL, NULL, n, axis, bv.obb.To, bv.obb.extent); // get center and extension const Vec3f& center = bv.obb.To; const Vec3f& extent = bv.obb.extent; FCL_REAL r0 = maximumDistance(ps, NULL, NULL, NULL, n, center); // decide the k in kIOS if(extent[0] > kIOS_RATIO * extent[2]) { if(extent[0] > kIOS_RATIO * extent[1]) bv.num_spheres = 5; else bv.num_spheres = 3; } else bv.num_spheres = 1; bv.spheres[0].o = center; bv.spheres[0].r = r0; if(bv.num_spheres >= 3) { FCL_REAL r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA; Vec3f delta = axis[2] * (r10 * cosA - extent[2]); bv.spheres[1].o = center - delta; bv.spheres[2].o = center + delta; FCL_REAL r11 = 0, r12 = 0; r11 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[1].o); r12 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[2].o); bv.spheres[1].o += axis[2] * (-r10 + r11); bv.spheres[2].o += axis[2] * (r10 - r12); bv.spheres[1].r = r10; bv.spheres[2].r = r10; } if(bv.num_spheres >= 5) { FCL_REAL r10 = bv.spheres[1].r; Vec3f delta = axis[1] * (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) - extent[1]); bv.spheres[3].o = bv.spheres[0].o - delta; bv.spheres[4].o = bv.spheres[0].o + delta; FCL_REAL r21 = 0, r22 = 0; r21 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[3].o); r22 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[4].o); bv.spheres[3].o += axis[1] * (-r10 + r21); bv.spheres[4].o += axis[1] * (r10 - r22); bv.spheres[3].r = r10; bv.spheres[4].r = r10; } } } namespace OBBRSS_fit_functions { void fit1(Vec3f* ps, OBBRSS& bv) { OBB_fit_functions::fit1(ps, bv.obb); RSS_fit_functions::fit1(ps, bv.rss); } void fit2(Vec3f* ps, OBBRSS& bv) { OBB_fit_functions::fit2(ps, bv.obb); RSS_fit_functions::fit2(ps, bv.rss); } void fit3(Vec3f* ps, OBBRSS& bv) { OBB_fit_functions::fit3(ps, bv.obb); RSS_fit_functions::fit3(ps, bv.rss); } void fitn(Vec3f* ps, int n, OBBRSS& bv) { OBB_fit_functions::fitn(ps, n, bv.obb); RSS_fit_functions::fitn(ps, n, bv.rss); } } template<> void fit(Vec3f* ps, int n, OBB& bv) { switch(n) { case 1: OBB_fit_functions::fit1(ps, bv); break; case 2: OBB_fit_functions::fit2(ps, bv); break; case 3: OBB_fit_functions::fit3(ps, bv); break; case 6: OBB_fit_functions::fit6(ps, bv); break; default: OBB_fit_functions::fitn(ps, n, bv); } } template<> void fit(Vec3f* ps, int n, RSS& bv) { switch(n) { case 1: RSS_fit_functions::fit1(ps, bv); break; case 2: RSS_fit_functions::fit2(ps, bv); break; case 3: RSS_fit_functions::fit3(ps, bv); break; default: RSS_fit_functions::fitn(ps, n, bv); } } template<> void fit(Vec3f* ps, int n, kIOS& bv) { switch(n) { case 1: kIOS_fit_functions::fit1(ps, bv); break; case 2: kIOS_fit_functions::fit2(ps, bv); break; case 3: kIOS_fit_functions::fit3(ps, bv); break; default: kIOS_fit_functions::fitn(ps, n, bv); } } template<> void fit(Vec3f* ps, int n, OBBRSS& bv) { switch(n) { case 1: OBBRSS_fit_functions::fit1(ps, bv); break; case 2: OBBRSS_fit_functions::fit2(ps, bv); break; case 3: OBBRSS_fit_functions::fit3(ps, bv); break; default: OBBRSS_fit_functions::fitn(ps, n, bv); } } OBB BVFitter::fit(unsigned int* primitive_indices, int num_primitives) { OBB bv; Matrix3f M; // row first matrix Vec3f E[3]; // row first eigen-vectors Matrix3f::U s[3]; // three eigen values getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M); eigen(M, s, E); axisFromEigen(E, s, bv.axis); // set obb centers and extensions getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.axis, bv.To, bv.extent); return bv; } OBBRSS BVFitter::fit(unsigned int* primitive_indices, int num_primitives) { OBBRSS bv; Matrix3f M; Vec3f E[3]; Matrix3f::U s[3]; getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M); eigen(M, s, E); axisFromEigen(E, s, bv.obb.axis); bv.rss.axis[0] = bv.obb.axis[0]; bv.rss.axis[1] = bv.obb.axis[1]; bv.rss.axis[2] = bv.obb.axis[2]; getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.obb.axis, bv.obb.To, bv.obb.extent); Vec3f origin; FCL_REAL l[2]; FCL_REAL r; getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.rss.axis, origin, l, r); bv.rss.Tr = origin; bv.rss.l[0] = l[0]; bv.rss.l[1] = l[1]; bv.rss.r = r; return bv; } RSS BVFitter::fit(unsigned int* primitive_indices, int num_primitives) { RSS bv; Matrix3f M; // row first matrix Vec3f E[3]; // row first eigen-vectors Matrix3f::U s[3]; // three eigen values getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M); eigen(M, s, E); axisFromEigen(E, s, bv.axis); // set rss origin, rectangle size and radius Vec3f origin; FCL_REAL l[2]; FCL_REAL r; getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.axis, origin, l, r); bv.Tr = origin; bv.l[0] = l[0]; bv.l[1] = l[1]; bv.r = r; return bv; } kIOS BVFitter::fit(unsigned int* primitive_indices, int num_primitives) { kIOS bv; Matrix3f M; // row first matrix Vec3f E[3]; // row first eigen-vectors Matrix3f::U s[3]; getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M); eigen(M, s, E); Vec3f* axis = bv.obb.axis; axisFromEigen(E, s, axis); // get centers and extensions getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, axis, bv.obb.To, bv.obb.extent); const Vec3f& center = bv.obb.To; const Vec3f& extent = bv.obb.extent; FCL_REAL r0 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, center); // decide k in kIOS if(extent[0] > kIOS_RATIO * extent[2]) { if(extent[0] > kIOS_RATIO * extent[1]) bv.num_spheres = 5; else bv.num_spheres = 3; } else bv.num_spheres = 1; bv.spheres[0].o = center; bv.spheres[0].r = r0; if(bv.num_spheres >= 3) { FCL_REAL r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA; Vec3f delta = axis[2] * (r10 * cosA - extent[2]); bv.spheres[1].o = center - delta; bv.spheres[2].o = center + delta; FCL_REAL r11 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[1].o); FCL_REAL r12 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[2].o); bv.spheres[1].o += axis[2] * (-r10 + r11); bv.spheres[2].o += axis[2] * (r10 - r12); bv.spheres[1].r = r10; bv.spheres[2].r = r10; } if(bv.num_spheres >= 5) { FCL_REAL r10 = bv.spheres[1].r; Vec3f delta = axis[1] * (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) - extent[1]); bv.spheres[3].o = bv.spheres[0].o - delta; bv.spheres[4].o = bv.spheres[0].o + delta; FCL_REAL r21 = 0, r22 = 0; r21 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[3].o); r22 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[4].o); bv.spheres[3].o += axis[1] * (-r10 + r21); bv.spheres[4].o += axis[1] * (r10 - r22); bv.spheres[3].r = r10; bv.spheres[4].r = r10; } return bv; } } fcl-0.5.0/src/BVH/BV_splitter.cpp000066400000000000000000000216571274356571000164540ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/BVH/BV_splitter.h" namespace fcl { template void computeSplitVector(const BV& bv, Vec3f& split_vector) { split_vector = bv.axis[0]; } template<> void computeSplitVector(const kIOS& bv, Vec3f& split_vector) { /* switch(bv.num_spheres) { case 1: split_vector = Vec3f(1, 0, 0); break; case 3: { Vec3f v[3]; v[0] = bv.spheres[1].o - bv.spheres[0].o; v[0].normalize(); generateCoordinateSystem(v[0], v[1], v[2]); split_vector = v[1]; } break; case 5: { Vec3f v[2]; v[0] = bv.spheres[1].o - bv.spheres[0].o; v[1] = bv.spheres[3].o - bv.spheres[0].o; split_vector = v[0].cross(v[1]); split_vector.normalize(); } break; default: ; } */ split_vector = bv.obb.axis[0]; } template<> void computeSplitVector(const OBBRSS& bv, Vec3f& split_vector) { split_vector = bv.obb.axis[0]; } template void computeSplitValue_bvcenter(const BV& bv, FCL_REAL& split_value) { Vec3f center = bv.center(); split_value = center[0]; } template void computeSplitValue_mean(const BV& bv, Vec3f* vertices, Triangle* triangles, unsigned int* primitive_indices, int num_primitives, BVHModelType type, const Vec3f& split_vector, FCL_REAL& split_value) { FCL_REAL sum = 0.0; if(type == BVH_MODEL_TRIANGLES) { FCL_REAL c[3] = {0.0, 0.0, 0.0}; for(int i = 0; i < num_primitives; ++i) { const Triangle& t = triangles[primitive_indices[i]]; const Vec3f& p1 = vertices[t[0]]; const Vec3f& p2 = vertices[t[1]]; const Vec3f& p3 = vertices[t[2]]; c[0] += (p1[0] + p2[0] + p3[0]); c[1] += (p1[1] + p2[1] + p3[1]); c[2] += (p1[2] + p2[2] + p3[2]); } split_value = (c[0] * split_vector[0] + c[1] * split_vector[1] + c[2] * split_vector[2]) / (3 * num_primitives); } else if(type == BVH_MODEL_POINTCLOUD) { for(int i = 0; i < num_primitives; ++i) { const Vec3f& p = vertices[primitive_indices[i]]; Vec3f v(p[0], p[1], p[2]); sum += v.dot(split_vector); } split_value = sum / num_primitives; } } template void computeSplitValue_median(const BV& bv, Vec3f* vertices, Triangle* triangles, unsigned int* primitive_indices, int num_primitives, BVHModelType type, const Vec3f& split_vector, FCL_REAL& split_value) { std::vector proj(num_primitives); if(type == BVH_MODEL_TRIANGLES) { for(int i = 0; i < num_primitives; ++i) { const Triangle& t = triangles[primitive_indices[i]]; const Vec3f& p1 = vertices[t[0]]; const Vec3f& p2 = vertices[t[1]]; const Vec3f& p3 = vertices[t[2]]; Vec3f centroid3(p1[0] + p2[0] + p3[0], p1[1] + p2[1] + p3[1], p1[2] + p2[2] + p3[2]); proj[i] = centroid3.dot(split_vector) / 3; } } else if(type == BVH_MODEL_POINTCLOUD) { for(int i = 0; i < num_primitives; ++i) { const Vec3f& p = vertices[primitive_indices[i]]; Vec3f v(p[0], p[1], p[2]); proj[i] = v.dot(split_vector); } } std::sort(proj.begin(), proj.end()); if(num_primitives % 2 == 1) { split_value = proj[(num_primitives - 1) / 2]; } else { split_value = (proj[num_primitives / 2] + proj[num_primitives / 2 - 1]) / 2; } } template<> void BVSplitter::computeRule_bvcenter(const OBB& bv, unsigned int* primitive_indices, int num_primitives) { computeSplitVector(bv, split_vector); computeSplitValue_bvcenter(bv, split_value); } template<> void BVSplitter::computeRule_mean(const OBB& bv, unsigned int* primitive_indices, int num_primitives) { computeSplitVector(bv, split_vector); computeSplitValue_mean(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value); } template<> void BVSplitter::computeRule_median(const OBB& bv, unsigned int* primitive_indices, int num_primitives) { computeSplitVector(bv, split_vector); computeSplitValue_median(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value); } template<> void BVSplitter::computeRule_bvcenter(const RSS& bv, unsigned int* primitive_indices, int num_primitives) { computeSplitVector(bv, split_vector); computeSplitValue_bvcenter(bv, split_value); } template<> void BVSplitter::computeRule_mean(const RSS& bv, unsigned int* primitive_indices, int num_primitives) { computeSplitVector(bv, split_vector); computeSplitValue_mean(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value); } template<> void BVSplitter::computeRule_median(const RSS& bv, unsigned int* primitive_indices, int num_primitives) { computeSplitVector(bv, split_vector); computeSplitValue_median(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value); } template<> void BVSplitter::computeRule_bvcenter(const kIOS& bv, unsigned int* primitive_indices, int num_primitives) { computeSplitVector(bv, split_vector); computeSplitValue_bvcenter(bv, split_value); } template<> void BVSplitter::computeRule_mean(const kIOS& bv, unsigned int* primitive_indices, int num_primitives) { computeSplitVector(bv, split_vector); computeSplitValue_mean(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value); } template<> void BVSplitter::computeRule_median(const kIOS& bv, unsigned int* primitive_indices, int num_primitives) { computeSplitVector(bv, split_vector); computeSplitValue_median(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value); } template<> void BVSplitter::computeRule_bvcenter(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives) { computeSplitVector(bv, split_vector); computeSplitValue_bvcenter(bv, split_value); } template<> void BVSplitter::computeRule_mean(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives) { computeSplitVector(bv, split_vector); computeSplitValue_mean(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value); } template<> void BVSplitter::computeRule_median(const OBBRSS& bv, unsigned int* primitive_indices, int num_primitives) { computeSplitVector(bv, split_vector); computeSplitValue_median(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value); } template<> bool BVSplitter::apply(const Vec3f& q) const { return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value; } template<> bool BVSplitter::apply(const Vec3f& q) const { return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value; } template<> bool BVSplitter::apply(const Vec3f& q) const { return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value; } template<> bool BVSplitter::apply(const Vec3f& q) const { return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value; } template class BVSplitter; template class BVSplitter; template class BVSplitter; template class BVSplitter; } fcl-0.5.0/src/CMakeLists.txt000066400000000000000000000024421274356571000156230ustar00rootroot00000000000000file(GLOB_RECURSE FCL_SOURCE_CODE ${CMAKE_CURRENT_SOURCE_DIR}/*.cpp) if(FCL_STATIC_LIBRARY) add_library(${PROJECT_NAME} STATIC ${FCL_HEADERS} ${FCL_SOURCE_CODE}) else() add_library(${PROJECT_NAME} SHARED ${FCL_HEADERS} ${FCL_SOURCE_CODE}) endif() set_target_properties(${PROJECT_NAME} PROPERTIES VERSION ${FCL_VERSION} SOVERSION ${FCL_ABI_VERSION}) target_link_libraries(${PROJECT_NAME} PUBLIC ${OCTOMAP_LIBRARIES} PRIVATE ${CCD_LIBRARIES} PRIVATE ${Boost_LIBRARIES}) target_include_directories(${PROJECT_NAME} INTERFACE $ $) export(TARGETS ${PROJECT_NAME} FILE "${PROJECT_BINARY_DIR}/${PROJECT_NAME}Config.cmake") install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}Config ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) if(WIN32 AND NOT CYGWIN) install(EXPORT ${PROJECT_NAME}Config DESTINATION CMake) else() install(EXPORT ${PROJECT_NAME}Config DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/${PROJECT_NAME}) endif() # Setup the coveralls target and tell it to gather coverage data for all the lib sources. if(FCL_COVERALLS) coveralls_setup("${FCL_HEADERS};${FCL_SOURCE_CODE};" ${FCL_COVERALLS_UPLOAD}) endif() fcl-0.5.0/src/articulated_model/000077500000000000000000000000001274356571000165425ustar00rootroot00000000000000fcl-0.5.0/src/articulated_model/joint.cpp000066400000000000000000000123171274356571000203750ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Dalibor Matura, Jia Pan */ #include "fcl/articulated_model/joint.h" #include "fcl/articulated_model/link.h" #include "fcl/articulated_model/joint_config.h" namespace fcl { Joint::Joint(const std::shared_ptr& link_parent, const std::shared_ptr& link_child, const Transform3f& transform_to_parent, const std::string& name) : link_parent_(link_parent), link_child_(link_child), name_(name), transform_to_parent_(transform_to_parent) {} Joint::Joint(const std::string& name) : name_(name) { } const std::string& Joint::getName() const { return name_; } void Joint::setName(const std::string& name) { name_ = name; } std::shared_ptr Joint::getJointConfig() const { return joint_cfg_; } void Joint::setJointConfig(const std::shared_ptr& joint_cfg) { joint_cfg_ = joint_cfg; } std::shared_ptr Joint::getParentLink() const { return link_parent_.lock(); } std::shared_ptr Joint::getChildLink() const { return link_child_.lock(); } void Joint::setParentLink(const std::shared_ptr& link) { link_parent_ = link; } void Joint::setChildLink(const std::shared_ptr& link) { link_child_ = link; } JointType Joint::getJointType() const { return type_; } const Transform3f& Joint::getTransformToParent() const { return transform_to_parent_; } void Joint::setTransformToParent(const Transform3f& t) { transform_to_parent_ = t; } PrismaticJoint::PrismaticJoint(const std::shared_ptr& link_parent, const std::shared_ptr& link_child, const Transform3f& transform_to_parent, const std::string& name, const Vec3f& axis) : Joint(link_parent, link_child, transform_to_parent, name), axis_(axis) { type_ = JT_PRISMATIC; } const Vec3f& PrismaticJoint::getAxis() const { return axis_; } std::size_t PrismaticJoint::getNumDofs() const { return 1; } Transform3f PrismaticJoint::getLocalTransform() const { const Quaternion3f& quat = transform_to_parent_.getQuatRotation(); const Vec3f& transl = transform_to_parent_.getTranslation(); return Transform3f(quat, quat.transform(axis_ * (*joint_cfg_)[0]) + transl); } RevoluteJoint::RevoluteJoint(const std::shared_ptr& link_parent, const std::shared_ptr& link_child, const Transform3f& transform_to_parent, const std::string& name, const Vec3f& axis) : Joint(link_parent, link_child, transform_to_parent, name), axis_(axis) { type_ = JT_REVOLUTE; } const Vec3f& RevoluteJoint::getAxis() const { return axis_; } std::size_t RevoluteJoint::getNumDofs() const { return 1; } Transform3f RevoluteJoint::getLocalTransform() const { Quaternion3f quat; quat.fromAxisAngle(axis_, (*joint_cfg_)[0]); return Transform3f(transform_to_parent_.getQuatRotation() * quat, transform_to_parent_.getTranslation()); } BallEulerJoint::BallEulerJoint(const std::shared_ptr& link_parent, const std::shared_ptr& link_child, const Transform3f& transform_to_parent, const std::string& name) : Joint(link_parent, link_child, transform_to_parent, name) {} std::size_t BallEulerJoint::getNumDofs() const { return 3; } Transform3f BallEulerJoint::getLocalTransform() const { Matrix3f rot; rot.setEulerYPR((*joint_cfg_)[0], (*joint_cfg_)[1], (*joint_cfg_)[2]); return transform_to_parent_ * Transform3f(rot); } } fcl-0.5.0/src/articulated_model/joint_config.cpp000066400000000000000000000061471274356571000217260ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Dalibor Matura, Jia Pan */ #include "fcl/articulated_model/joint_config.h" #include "fcl/articulated_model/joint.h" namespace fcl { JointConfig::JointConfig() {} JointConfig::JointConfig(const JointConfig& joint_cfg) : joint_(joint_cfg.joint_), values_(joint_cfg.values_), limits_min_(joint_cfg.limits_min_), limits_max_(joint_cfg.limits_max_) { } JointConfig::JointConfig(const std::shared_ptr& joint, FCL_REAL default_value, FCL_REAL default_value_min, FCL_REAL default_value_max) : joint_(joint) { values_.resize(joint->getNumDofs(), default_value); limits_min_.resize(joint->getNumDofs(), default_value_min); limits_max_.resize(joint->getNumDofs(), default_value_max); } std::size_t JointConfig::getDim() const { return values_.size(); } FCL_REAL JointConfig::getValue(std::size_t i) const { return values_[i]; } FCL_REAL& JointConfig::getValue(std::size_t i) { return values_[i]; } FCL_REAL JointConfig::getLimitMin(std::size_t i) const { return limits_min_[i]; } FCL_REAL& JointConfig::getLimitMin(std::size_t i) { return limits_min_[i]; } FCL_REAL JointConfig::getLimitMax(std::size_t i) const { return limits_max_[i]; } FCL_REAL& JointConfig::getLimitMax(std::size_t i) { return limits_max_[i]; } std::shared_ptr JointConfig::getJoint() const { return joint_.lock(); } } fcl-0.5.0/src/articulated_model/link.cpp000066400000000000000000000047361274356571000202150ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Dalibor Matura, Jia Pan */ #include "fcl/articulated_model/link.h" #include "fcl/articulated_model/joint.h" namespace fcl { Link::Link(const std::string& name) : name_(name) {} const std::string& Link::getName() const { return name_; } void Link::setName(const std::string& name) { name_ = name; } void Link::addChildJoint(const std::shared_ptr& joint) { children_joints_.push_back(joint); } void Link::setParentJoint(const std::shared_ptr& joint) { parent_joint_ = joint; } void Link::addObject(const std::shared_ptr& object) { objects_.push_back(object); } std::size_t Link::getNumChildJoints() const { return children_joints_.size(); } std::size_t Link::getNumObjects() const { return objects_.size(); } } fcl-0.5.0/src/articulated_model/model.cpp000066400000000000000000000111311274356571000203430ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Dalibor Matura, Jia Pan */ #include "fcl/articulated_model/model.h" #include "fcl/articulated_model/model_config.h" namespace fcl { std::shared_ptr Model::getRoot() const { return root_link_; } std::shared_ptr Model::getLink(const std::string& name) const { std::shared_ptr ptr; std::map >::const_iterator it = links_.find(name); if(it == links_.end()) ptr.reset(); else ptr = it->second; return ptr; } std::shared_ptr Model::getJoint(const std::string& name) const { std::shared_ptr ptr; std::map >::const_iterator it = joints_.find(name); if(it == joints_.end()) ptr.reset(); else ptr = it->second; return ptr; } const std::string& Model::getName() const { return name_; } std::vector > Model::getLinks() const { std::vector > links; for(std::map >::const_iterator it = links_.begin(); it != links_.end(); ++it) { links.push_back(it->second); } return links; } std::size_t Model::getNumLinks() const { return links_.size(); } std::size_t Model::getNumJoints() const { return joints_.size(); } std::size_t Model::getNumDofs() const { std::size_t dof = 0; for(std::map >::const_iterator it = joints_.begin(); it != joints_.end(); ++it) { dof += it->second->getNumDofs(); } return dof; } void Model::addLink(const std::shared_ptr& link) { links_[link->getName()] = link; } void Model::addJoint(const std::shared_ptr& joint) { joints_[joint->getName()] = joint; } void Model::initRoot(const std::map& link_parent_tree) { root_link_.reset(); /// find the links that have no parent in the tree for(std::map >::const_iterator it = links_.begin(); it != links_.end(); ++it) { std::map::const_iterator parent = link_parent_tree.find(it->first); if(parent == link_parent_tree.end()) { if(!root_link_) { root_link_ = getLink(it->first); } else { throw ModelParseError("Two root links found: [" + root_link_->getName() + "] and [" + it->first + "]"); } } } if(!root_link_) throw ModelParseError("No root link found."); } void Model::initTree(std::map& link_parent_tree) { for(std::map >::iterator it = joints_.begin(); it != joints_.end(); ++it) { std::string parent_link_name = it->second->getParentLink()->getName(); std::string child_link_name = it->second->getChildLink()->getName(); it->second->getParentLink()->addChildJoint(it->second); it->second->getChildLink()->setParentJoint(it->second); link_parent_tree[child_link_name] = parent_link_name; } } } fcl-0.5.0/src/articulated_model/model_config.cpp000066400000000000000000000060411274356571000216740ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Dalibor Matura, Jia Pan */ #include "fcl/articulated_model/model_config.h" #include "fcl/articulated_model/joint.h" #include #include namespace fcl { ModelConfig::ModelConfig() {} ModelConfig::ModelConfig(const ModelConfig& model_cfg) : joint_cfgs_map_(model_cfg.joint_cfgs_map_) {} ModelConfig::ModelConfig(std::map > joints_map) { std::map >::iterator it; for(it = joints_map.begin(); it != joints_map.end(); ++it) joint_cfgs_map_[it->first] = JointConfig(it->second); } JointConfig ModelConfig::getJointConfigByJointName(const std::string& joint_name) const { std::map::const_iterator it = joint_cfgs_map_.find(joint_name); assert(it != joint_cfgs_map_.end()); return it->second; } JointConfig& ModelConfig::getJointConfigByJointName(const std::string& joint_name) { std::map::iterator it = joint_cfgs_map_.find(joint_name); assert(it != joint_cfgs_map_.end()); return it->second; } JointConfig ModelConfig::getJointConfigByJoint(std::shared_ptr joint) const { return getJointConfigByJointName(joint->getName()); } JointConfig& ModelConfig::getJointConfigByJoint(std::shared_ptr joint) { return getJointConfigByJointName(joint->getName()); } } fcl-0.5.0/src/broadphase/000077500000000000000000000000001274356571000151715ustar00rootroot00000000000000fcl-0.5.0/src/broadphase/broadphase_SSaP.cpp000066400000000000000000000351301274356571000206750ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/broadphase/broadphase_SSaP.h" #include #include namespace fcl { /** \brief Functor sorting objects according to the AABB lower x bound */ struct SortByXLow { bool operator()(const CollisionObject* a, const CollisionObject* b) const { if(a->getAABB().min_[0] < b->getAABB().min_[0]) return true; return false; } }; /** \brief Functor sorting objects according to the AABB lower y bound */ struct SortByYLow { bool operator()(const CollisionObject* a, const CollisionObject* b) const { if(a->getAABB().min_[1] < b->getAABB().min_[1]) return true; return false; } }; /** \brief Functor sorting objects according to the AABB lower z bound */ struct SortByZLow { bool operator()(const CollisionObject* a, const CollisionObject* b) const { if(a->getAABB().min_[2] < b->getAABB().min_[2]) return true; return false; } }; /** \brief Dummy collision object with a point AABB */ class DummyCollisionObject : public CollisionObject { public: DummyCollisionObject(const AABB& aabb_) : CollisionObject(std::shared_ptr()) { aabb = aabb_; } void computeLocalAABB() {} }; void SSaPCollisionManager::unregisterObject(CollisionObject* obj) { setup(); DummyCollisionObject dummyHigh(AABB(obj->getAABB().max_)); std::vector::iterator pos_start1 = objs_x.begin(); std::vector::iterator pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow()); while(pos_start1 < pos_end1) { if(*pos_start1 == obj) { objs_x.erase(pos_start1); break; } ++pos_start1; } std::vector::iterator pos_start2 = objs_y.begin(); std::vector::iterator pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow()); while(pos_start2 < pos_end2) { if(*pos_start2 == obj) { objs_y.erase(pos_start2); break; } ++pos_start2; } std::vector::iterator pos_start3 = objs_z.begin(); std::vector::iterator pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow()); while(pos_start3 < pos_end3) { if(*pos_start3 == obj) { objs_z.erase(pos_start3); break; } ++pos_start3; } } void SSaPCollisionManager::registerObject(CollisionObject* obj) { objs_x.push_back(obj); objs_y.push_back(obj); objs_z.push_back(obj); setup_ = false; } void SSaPCollisionManager::setup() { if(!setup_) { std::sort(objs_x.begin(), objs_x.end(), SortByXLow()); std::sort(objs_y.begin(), objs_y.end(), SortByYLow()); std::sort(objs_z.begin(), objs_z.end(), SortByZLow()); setup_ = true; } } void SSaPCollisionManager::update() { setup_ = false; setup(); } void SSaPCollisionManager::clear() { objs_x.clear(); objs_y.clear(); objs_z.clear(); setup_ = false; } void SSaPCollisionManager::getObjects(std::vector& objs) const { objs.resize(objs_x.size()); std::copy(objs_x.begin(), objs_x.end(), objs.begin()); } bool SSaPCollisionManager::checkColl(std::vector::const_iterator pos_start, std::vector::const_iterator pos_end, CollisionObject* obj, void* cdata, CollisionCallBack callback) const { while(pos_start < pos_end) { if(*pos_start != obj) // no collision between the same object { if((*pos_start)->getAABB().overlap(obj->getAABB())) { if(callback(*pos_start, obj, cdata)) return true; } } pos_start++; } return false; } bool SSaPCollisionManager::checkDis(std::vector::const_iterator pos_start, std::vector::const_iterator pos_end, CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const { while(pos_start < pos_end) { if(*pos_start != obj) // no distance between the same object { if((*pos_start)->getAABB().distance(obj->getAABB()) < min_dist) { if(callback(*pos_start, obj, cdata, min_dist)) return true; } } pos_start++; } return false; } void SSaPCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const { if(size() == 0) return; collide_(obj, cdata, callback); } bool SSaPCollisionManager::collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const { static const unsigned int CUTOFF = 100; DummyCollisionObject dummyHigh(AABB(obj->getAABB().max_)); bool coll_res = false; std::vector::const_iterator pos_start1 = objs_x.begin(); std::vector::const_iterator pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow()); unsigned int d1 = pos_end1 - pos_start1; if(d1 > CUTOFF) { std::vector::const_iterator pos_start2 = objs_y.begin(); std::vector::const_iterator pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow()); unsigned int d2 = pos_end2 - pos_start2; if(d2 > CUTOFF) { std::vector::const_iterator pos_start3 = objs_z.begin(); std::vector::const_iterator pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow()); unsigned int d3 = pos_end3 - pos_start3; if(d3 > CUTOFF) { if(d3 <= d2 && d3 <= d1) coll_res = checkColl(pos_start3, pos_end3, obj, cdata, callback); else { if(d2 <= d3 && d2 <= d1) coll_res = checkColl(pos_start2, pos_end2, obj, cdata, callback); else coll_res = checkColl(pos_start1, pos_end1, obj, cdata, callback); } } else coll_res = checkColl(pos_start3, pos_end3, obj, cdata, callback); } else coll_res = checkColl(pos_start2, pos_end2, obj, cdata, callback); } else coll_res = checkColl(pos_start1, pos_end1, obj, cdata, callback); return coll_res; } void SSaPCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const { if(size() == 0) return; FCL_REAL min_dist = std::numeric_limits::max(); distance_(obj, cdata, callback, min_dist); } bool SSaPCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const { static const unsigned int CUTOFF = 100; Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; Vec3f dummy_vector = obj->getAABB().max_; if(min_dist < std::numeric_limits::max()) dummy_vector += Vec3f(min_dist, min_dist, min_dist); std::vector::const_iterator pos_start1 = objs_x.begin(); std::vector::const_iterator pos_start2 = objs_y.begin(); std::vector::const_iterator pos_start3 = objs_z.begin(); std::vector::const_iterator pos_end1 = objs_x.end(); std::vector::const_iterator pos_end2 = objs_y.end(); std::vector::const_iterator pos_end3 = objs_z.end(); int status = 1; FCL_REAL old_min_distance; while(1) { old_min_distance = min_dist; DummyCollisionObject dummyHigh((AABB(dummy_vector))); pos_end1 = std::upper_bound(pos_start1, objs_x.end(), &dummyHigh, SortByXLow()); unsigned int d1 = pos_end1 - pos_start1; bool dist_res = false; if(d1 > CUTOFF) { pos_end2 = std::upper_bound(pos_start2, objs_y.end(), &dummyHigh, SortByYLow()); unsigned int d2 = pos_end2 - pos_start2; if(d2 > CUTOFF) { pos_end3 = std::upper_bound(pos_start3, objs_z.end(), &dummyHigh, SortByZLow()); unsigned int d3 = pos_end3 - pos_start3; if(d3 > CUTOFF) { if(d3 <= d2 && d3 <= d1) dist_res = checkDis(pos_start3, pos_end3, obj, cdata, callback, min_dist); else { if(d2 <= d3 && d2 <= d1) dist_res = checkDis(pos_start2, pos_end2, obj, cdata, callback, min_dist); else dist_res = checkDis(pos_start1, pos_end1, obj, cdata, callback, min_dist); } } else dist_res = checkDis(pos_start3, pos_end3, obj, cdata, callback, min_dist); } else dist_res = checkDis(pos_start2, pos_end2, obj, cdata, callback, min_dist); } else { dist_res = checkDis(pos_start1, pos_end1, obj, cdata, callback, min_dist); } if(dist_res) return true; if(status == 1) { if(old_min_distance < std::numeric_limits::max()) break; else { // from infinity to a finite one, only need one additional loop // to check the possible missed ones to the right of the objs array if(min_dist < old_min_distance) { dummy_vector = obj->getAABB().max_ + Vec3f(min_dist, min_dist, min_dist); status = 0; } else // need more loop { if(dummy_vector.equal(obj->getAABB().max_)) dummy_vector = dummy_vector + delta; else dummy_vector = dummy_vector * 2 - obj->getAABB().max_; } } // yes, following is wrong, will result in too large distance. // if(pos_end1 != objs_x.end()) pos_start1 = pos_end1; // if(pos_end2 != objs_y.end()) pos_start2 = pos_end2; // if(pos_end3 != objs_z.end()) pos_start3 = pos_end3; } else if(status == 0) break; } return false; } void SSaPCollisionManager::collide(void* cdata, CollisionCallBack callback) const { if(size() == 0) return; std::vector::const_iterator pos, run_pos, pos_end; size_t axis = selectOptimalAxis(objs_x, objs_y, objs_z, pos, pos_end); size_t axis2 = (axis + 1 > 2) ? 0 : (axis + 1); size_t axis3 = (axis2 + 1 > 2) ? 0 : (axis2 + 1); run_pos = pos; while((run_pos < pos_end) && (pos < pos_end)) { CollisionObject* obj = *(pos++); while(1) { if((*run_pos)->getAABB().min_[axis] < obj->getAABB().min_[axis]) { run_pos++; if(run_pos == pos_end) break; continue; } else { run_pos++; break; } } if(run_pos < pos_end) { std::vector::const_iterator run_pos2 = run_pos; while((*run_pos2)->getAABB().min_[axis] <= obj->getAABB().max_[axis]) { CollisionObject* obj2 = *run_pos2; run_pos2++; if((obj->getAABB().max_[axis2] >= obj2->getAABB().min_[axis2]) && (obj2->getAABB().max_[axis2] >= obj->getAABB().min_[axis2])) { if((obj->getAABB().max_[axis3] >= obj2->getAABB().min_[axis3]) && (obj2->getAABB().max_[axis3] >= obj->getAABB().min_[axis3])) { if(callback(obj, obj2, cdata)) return; } } if(run_pos2 == pos_end) break; } } } } void SSaPCollisionManager::distance(void* cdata, DistanceCallBack callback) const { if(size() == 0) return; std::vector::const_iterator it, it_end; selectOptimalAxis(objs_x, objs_y, objs_z, it, it_end); FCL_REAL min_dist = std::numeric_limits::max(); for(; it != it_end; ++it) { if(distance_(*it, cdata, callback, min_dist)) return; } } void SSaPCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const { SSaPCollisionManager* other_manager = static_cast(other_manager_); if((size() == 0) || (other_manager->size() == 0)) return; if(this == other_manager) { collide(cdata, callback); return; } std::vector::const_iterator it, end; if(this->size() < other_manager->size()) { for(it = objs_x.begin(), end = objs_x.end(); it != end; ++it) if(other_manager->collide_(*it, cdata, callback)) return; } else { for(it = other_manager->objs_x.begin(), end = other_manager->objs_x.end(); it != end; ++it) if(collide_(*it, cdata, callback)) return; } } void SSaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const { SSaPCollisionManager* other_manager = static_cast(other_manager_); if((size() == 0) || (other_manager->size() == 0)) return; if(this == other_manager) { distance(cdata, callback); return; } FCL_REAL min_dist = std::numeric_limits::max(); std::vector::const_iterator it, end; if(this->size() < other_manager->size()) { for(it = objs_x.begin(), end = objs_x.end(); it != end; ++it) if(other_manager->distance_(*it, cdata, callback, min_dist)) return; } else { for(it = other_manager->objs_x.begin(), end = other_manager->objs_x.end(); it != end; ++it) if(distance_(*it, cdata, callback, min_dist)) return; } } bool SSaPCollisionManager::empty() const { return objs_x.empty(); } } fcl-0.5.0/src/broadphase/broadphase_SaP.cpp000066400000000000000000000535601274356571000205610ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/broadphase/broadphase_SaP.h" #include #include #include namespace fcl { void SaPCollisionManager::unregisterObject(CollisionObject* obj) { std::list::iterator it = AABB_arr.begin(); for(std::list::iterator end = AABB_arr.end(); it != end; ++it) { if((*it)->obj == obj) break; } AABB_arr.erase(it); obj_aabb_map.erase(obj); if(it == AABB_arr.end()) return; SaPAABB* curr = *it; *it = NULL; for(int coord = 0; coord < 3; ++coord) { //first delete the lo endpoint of the interval. if(curr->lo->prev[coord] == NULL) elist[coord] = curr->lo->next[coord]; else curr->lo->prev[coord]->next[coord] = curr->lo->next[coord]; curr->lo->next[coord]->prev[coord] = curr->lo->prev[coord]; //then, delete the "hi" endpoint. if(curr->hi->prev[coord] == NULL) elist[coord] = curr->hi->next[coord]; else curr->hi->prev[coord]->next[coord] = curr->hi->next[coord]; if(curr->hi->next[coord] != NULL) curr->hi->next[coord]->prev[coord] = curr->hi->prev[coord]; } delete curr->lo; delete curr->hi; delete curr; overlap_pairs.remove_if(isUnregistered(obj)); } void SaPCollisionManager::registerObjects(const std::vector& other_objs) { if(other_objs.empty()) return; if(size() > 0) BroadPhaseCollisionManager::registerObjects(other_objs); else { std::vector endpoints(2 * other_objs.size()); for(size_t i = 0; i < other_objs.size(); ++i) { SaPAABB* sapaabb = new SaPAABB(); sapaabb->obj = other_objs[i]; sapaabb->lo = new EndPoint(); sapaabb->hi = new EndPoint(); sapaabb->cached = other_objs[i]->getAABB(); endpoints[2 * i] = sapaabb->lo; endpoints[2 * i + 1] = sapaabb->hi; sapaabb->lo->minmax = 0; sapaabb->hi->minmax = 1; sapaabb->lo->aabb = sapaabb; sapaabb->hi->aabb = sapaabb; AABB_arr.push_back(sapaabb); obj_aabb_map[other_objs[i]] = sapaabb; } FCL_REAL scale[3]; for(size_t coord = 0; coord < 3; ++coord) { std::sort(endpoints.begin(), endpoints.end(), std::bind(std::less(), std::bind(static_cast(&EndPoint::getVal), std::placeholders::_1, coord), std::bind(static_cast(&EndPoint::getVal), std::placeholders::_2, coord))); endpoints[0]->prev[coord] = NULL; endpoints[0]->next[coord] = endpoints[1]; for(size_t i = 1; i < endpoints.size() - 1; ++i) { endpoints[i]->prev[coord] = endpoints[i-1]; endpoints[i]->next[coord] = endpoints[i+1]; } endpoints[endpoints.size() - 1]->prev[coord] = endpoints[endpoints.size() - 2]; endpoints[endpoints.size() - 1]->next[coord] = NULL; elist[coord] = endpoints[0]; scale[coord] = endpoints.back()->aabb->cached.max_[coord] - endpoints[0]->aabb->cached.min_[coord]; } int axis = 0; if(scale[axis] < scale[1]) axis = 1; if(scale[axis] < scale[2]) axis = 2; EndPoint* pos = elist[axis]; while(pos != NULL) { EndPoint* pos_next = NULL; SaPAABB* aabb = pos->aabb; EndPoint* pos_it = pos->next[axis]; while(pos_it != NULL) { if(pos_it->aabb == aabb) { if(pos_next == NULL) pos_next = pos_it; break; } if(pos_it->minmax == 0) { if(pos_next == NULL) pos_next = pos_it; if(pos_it->aabb->cached.overlap(aabb->cached)) overlap_pairs.push_back(SaPPair(pos_it->aabb->obj, aabb->obj)); } pos_it = pos_it->next[axis]; } pos = pos_next; } } updateVelist(); } void SaPCollisionManager::registerObject(CollisionObject* obj) { SaPAABB* curr = new SaPAABB; curr->cached = obj->getAABB(); curr->obj = obj; curr->lo = new EndPoint; curr->lo->minmax = 0; curr->lo->aabb = curr; curr->hi = new EndPoint; curr->hi->minmax = 1; curr->hi->aabb = curr; for(int coord = 0; coord < 3; ++coord) { EndPoint* current = elist[coord]; // first insert the lo end point if(current == NULL) // empty list { elist[coord] = curr->lo; curr->lo->prev[coord] = curr->lo->next[coord] = NULL; } else // otherwise, find the correct location in the list and insert { EndPoint* curr_lo = curr->lo; FCL_REAL curr_lo_val = curr_lo->getVal()[coord]; while((current->getVal()[coord] < curr_lo_val) && (current->next[coord] != NULL)) current = current->next[coord]; if(current->getVal()[coord] >= curr_lo_val) { curr_lo->prev[coord] = current->prev[coord]; curr_lo->next[coord] = current; if(current->prev[coord] == NULL) elist[coord] = curr_lo; else current->prev[coord]->next[coord] = curr_lo; current->prev[coord] = curr_lo; } else { curr_lo->prev[coord] = current; curr_lo->next[coord] = NULL; current->next[coord] = curr_lo; } } // now insert hi end point current = curr->lo; EndPoint* curr_hi = curr->hi; FCL_REAL curr_hi_val = curr_hi->getVal()[coord]; if(coord == 0) { while((current->getVal()[coord] < curr_hi_val) && (current->next[coord] != NULL)) { if(current != curr->lo) if(current->aabb->cached.overlap(curr->cached)) overlap_pairs.push_back(SaPPair(current->aabb->obj, obj)); current = current->next[coord]; } } else { while((current->getVal()[coord] < curr_hi_val) && (current->next[coord] != NULL)) current = current->next[coord]; } if(current->getVal()[coord] >= curr_hi_val) { curr_hi->prev[coord] = current->prev[coord]; curr_hi->next[coord] = current; if(current->prev[coord] == NULL) elist[coord] = curr_hi; else current->prev[coord]->next[coord] = curr_hi; current->prev[coord] = curr_hi; } else { curr_hi->prev[coord] = current; curr_hi->next[coord] = NULL; current->next[coord] = curr_hi; } } AABB_arr.push_back(curr); obj_aabb_map[obj] = curr; updateVelist(); } void SaPCollisionManager::setup() { if(size() == 0) return; FCL_REAL scale[3]; scale[0] = (velist[0].back())->getVal(0) - velist[0][0]->getVal(0); scale[1] = (velist[1].back())->getVal(1) - velist[1][0]->getVal(1);; scale[2] = (velist[2].back())->getVal(2) - velist[2][0]->getVal(2); size_t axis = 0; if(scale[axis] < scale[1]) axis = 1; if(scale[axis] < scale[2]) axis = 2; optimal_axis = axis; } void SaPCollisionManager::update_(SaPAABB* updated_aabb) { if(updated_aabb->cached.equal(updated_aabb->obj->getAABB())) return; SaPAABB* current = updated_aabb; Vec3f new_min = current->obj->getAABB().min_; Vec3f new_max = current->obj->getAABB().max_; SaPAABB dummy; dummy.cached = current->obj->getAABB(); for(int coord = 0; coord < 3; ++coord) { int direction; // -1 reverse, 0 nochange, 1 forward EndPoint* temp; if(current->lo->getVal(coord) > new_min[coord]) direction = -1; else if(current->lo->getVal(coord) < new_min[coord]) direction = 1; else direction = 0; if(direction == -1) { //first update the "lo" endpoint of the interval if(current->lo->prev[coord] != NULL) { temp = current->lo; while((temp != NULL) && (temp->getVal(coord) > new_min[coord])) { if(temp->minmax == 1) if(temp->aabb->cached.overlap(dummy.cached)) addToOverlapPairs(SaPPair(temp->aabb->obj, current->obj)); temp = temp->prev[coord]; } if(temp == NULL) { current->lo->prev[coord]->next[coord] = current->lo->next[coord]; current->lo->next[coord]->prev[coord] = current->lo->prev[coord]; current->lo->prev[coord] = NULL; current->lo->next[coord] = elist[coord]; elist[coord]->prev[coord] = current->lo; elist[coord] = current->lo; } else { current->lo->prev[coord]->next[coord] = current->lo->next[coord]; current->lo->next[coord]->prev[coord] = current->lo->prev[coord]; current->lo->prev[coord] = temp; current->lo->next[coord] = temp->next[coord]; temp->next[coord]->prev[coord] = current->lo; temp->next[coord] = current->lo; } } current->lo->getVal(coord) = new_min[coord]; // update hi end point temp = current->hi; while(temp->getVal(coord) > new_max[coord]) { if((temp->minmax == 0) && (temp->aabb->cached.overlap(current->cached))) removeFromOverlapPairs(SaPPair(temp->aabb->obj, current->obj)); temp = temp->prev[coord]; } current->hi->prev[coord]->next[coord] = current->hi->next[coord]; if(current->hi->next[coord] != NULL) current->hi->next[coord]->prev[coord] = current->hi->prev[coord]; current->hi->prev[coord] = temp; current->hi->next[coord] = temp->next[coord]; if(temp->next[coord] != NULL) temp->next[coord]->prev[coord] = current->hi; temp->next[coord] = current->hi; current->hi->getVal(coord) = new_max[coord]; } else if(direction == 1) { //here, we first update the "hi" endpoint. if(current->hi->next[coord] != NULL) { temp = current->hi; while((temp->next[coord] != NULL) && (temp->getVal(coord) < new_max[coord])) { if(temp->minmax == 0) if(temp->aabb->cached.overlap(dummy.cached)) addToOverlapPairs(SaPPair(temp->aabb->obj, current->obj)); temp = temp->next[coord]; } if(temp->getVal(coord) < new_max[coord]) { current->hi->prev[coord]->next[coord] = current->hi->next[coord]; current->hi->next[coord]->prev[coord] = current->hi->prev[coord]; current->hi->prev[coord] = temp; current->hi->next[coord] = NULL; temp->next[coord] = current->hi; } else { current->hi->prev[coord]->next[coord] = current->hi->next[coord]; current->hi->next[coord]->prev[coord] = current->hi->prev[coord]; current->hi->prev[coord] = temp->prev[coord]; current->hi->next[coord] = temp; temp->prev[coord]->next[coord] = current->hi; temp->prev[coord] = current->hi; } } current->hi->getVal(coord) = new_max[coord]; //then, update the "lo" endpoint of the interval. temp = current->lo; while(temp->getVal(coord) < new_min[coord]) { if((temp->minmax == 1) && (temp->aabb->cached.overlap(current->cached))) removeFromOverlapPairs(SaPPair(temp->aabb->obj, current->obj)); temp = temp->next[coord]; } if(current->lo->prev[coord] != NULL) current->lo->prev[coord]->next[coord] = current->lo->next[coord]; else elist[coord] = current->lo->next[coord]; current->lo->next[coord]->prev[coord] = current->lo->prev[coord]; current->lo->prev[coord] = temp->prev[coord]; current->lo->next[coord] = temp; if(temp->prev[coord] != NULL) temp->prev[coord]->next[coord] = current->lo; else elist[coord] = current->lo; temp->prev[coord] = current->lo; current->lo->getVal(coord) = new_min[coord]; } } } void SaPCollisionManager::update(CollisionObject* updated_obj) { update_(obj_aabb_map[updated_obj]); updateVelist(); setup(); } void SaPCollisionManager::update(const std::vector& updated_objs) { for(size_t i = 0; i < updated_objs.size(); ++i) update_(obj_aabb_map[updated_objs[i]]); updateVelist(); setup(); } void SaPCollisionManager::update() { for(std::list::const_iterator it = AABB_arr.begin(), end = AABB_arr.end(); it != end; ++it) { update_(*it); } updateVelist(); setup(); } void SaPCollisionManager::clear() { for(std::list::iterator it = AABB_arr.begin(), end = AABB_arr.end(); it != end; ++it) { delete (*it)->hi; delete (*it)->lo; delete *it; *it = NULL; } AABB_arr.clear(); overlap_pairs.clear(); elist[0] = NULL; elist[1] = NULL; elist[2] = NULL; velist[0].clear(); velist[1].clear(); velist[2].clear(); obj_aabb_map.clear(); } void SaPCollisionManager::getObjects(std::vector& objs) const { objs.resize(AABB_arr.size()); int i = 0; for(std::list::const_iterator it = AABB_arr.begin(), end = AABB_arr.end(); it != end; ++it, ++i) { objs[i] = (*it)->obj; } } bool SaPCollisionManager::collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const { size_t axis = optimal_axis; const AABB& obj_aabb = obj->getAABB(); FCL_REAL min_val = obj_aabb.min_[axis]; // FCL_REAL max_val = obj_aabb.max_[axis]; EndPoint dummy; SaPAABB dummy_aabb; dummy_aabb.cached = obj_aabb; dummy.minmax = 1; dummy.aabb = &dummy_aabb; // compute stop_pos by binary search, this is cheaper than check it in while iteration linearly std::vector::const_iterator res_it = std::upper_bound(velist[axis].begin(), velist[axis].end(), &dummy, std::bind(std::less(), std::bind(static_cast(&EndPoint::getVal), std::placeholders::_1, axis), std::bind(static_cast(&EndPoint::getVal), std::placeholders::_2, axis))); EndPoint* end_pos = NULL; if(res_it != velist[axis].end()) end_pos = *res_it; EndPoint* pos = elist[axis]; while(pos != end_pos) { if(pos->aabb->obj != obj) { if((pos->minmax == 0) && (pos->aabb->hi->getVal(axis) >= min_val)) { if(pos->aabb->cached.overlap(obj->getAABB())) if(callback(obj, pos->aabb->obj, cdata)) return true; } } pos = pos->next[axis]; } return false; } void SaPCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const { if(size() == 0) return; collide_(obj, cdata, callback); } bool SaPCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const { Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; AABB aabb = obj->getAABB(); if(min_dist < std::numeric_limits::max()) { Vec3f min_dist_delta(min_dist, min_dist, min_dist); aabb.expand(min_dist_delta); } size_t axis = optimal_axis; int status = 1; FCL_REAL old_min_distance; EndPoint* start_pos = elist[axis]; while(1) { old_min_distance = min_dist; FCL_REAL min_val = aabb.min_[axis]; // FCL_REAL max_val = aabb.max_[axis]; EndPoint dummy; SaPAABB dummy_aabb; dummy_aabb.cached = aabb; dummy.minmax = 1; dummy.aabb = &dummy_aabb; std::vector::const_iterator res_it = std::upper_bound(velist[axis].begin(), velist[axis].end(), &dummy, std::bind(std::less(), std::bind(static_cast(&EndPoint::getVal), std::placeholders::_1, axis), std::bind(static_cast(&EndPoint::getVal), std::placeholders::_2, axis))); EndPoint* end_pos = NULL; if(res_it != velist[axis].end()) end_pos = *res_it; EndPoint* pos = start_pos; while(pos != end_pos) { // can change to pos->aabb->hi->getVal(axis) >= min_val - min_dist, and then update start_pos to end_pos. // but this seems slower. if((pos->minmax == 0) && (pos->aabb->hi->getVal(axis) >= min_val)) { CollisionObject* curr_obj = pos->aabb->obj; if(curr_obj != obj) { if(!enable_tested_set_) { if(pos->aabb->cached.distance(obj->getAABB()) < min_dist) { if(callback(curr_obj, obj, cdata, min_dist)) return true; } } else { if(!inTestedSet(curr_obj, obj)) { if(pos->aabb->cached.distance(obj->getAABB()) < min_dist) { if(callback(curr_obj, obj, cdata, min_dist)) return true; } insertTestedSet(curr_obj, obj); } } } } pos = pos->next[axis]; } if(status == 1) { if(old_min_distance < std::numeric_limits::max()) break; else { if(min_dist < old_min_distance) { Vec3f min_dist_delta(min_dist, min_dist, min_dist); aabb = AABB(obj->getAABB(), min_dist_delta); status = 0; } else { if(aabb.equal(obj->getAABB())) aabb.expand(delta); else aabb.expand(obj->getAABB(), 2.0); } } } else if(status == 0) break; } return false; } void SaPCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const { if(size() == 0) return; FCL_REAL min_dist = std::numeric_limits::max(); distance_(obj, cdata, callback, min_dist); } void SaPCollisionManager::collide(void* cdata, CollisionCallBack callback) const { if(size() == 0) return; for(std::list::const_iterator it = overlap_pairs.begin(), end = overlap_pairs.end(); it != end; ++it) { CollisionObject* obj1 = it->obj1; CollisionObject* obj2 = it->obj2; if(callback(obj1, obj2, cdata)) return; } } void SaPCollisionManager::distance(void* cdata, DistanceCallBack callback) const { if(size() == 0) return; enable_tested_set_ = true; tested_set.clear(); FCL_REAL min_dist = std::numeric_limits::max(); for(std::list::const_iterator it = AABB_arr.begin(), end = AABB_arr.end(); it != end; ++it) { if(distance_((*it)->obj, cdata, callback, min_dist)) break; } enable_tested_set_ = false; tested_set.clear(); } void SaPCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const { SaPCollisionManager* other_manager = static_cast(other_manager_); if((size() == 0) || (other_manager->size() == 0)) return; if(this == other_manager) { collide(cdata, callback); return; } if(this->size() < other_manager->size()) { for(std::list::const_iterator it = AABB_arr.begin(); it != AABB_arr.end(); ++it) { if(other_manager->collide_((*it)->obj, cdata, callback)) return; } } else { for(std::list::const_iterator it = other_manager->AABB_arr.begin(), end = other_manager->AABB_arr.end(); it != end; ++it) { if(collide_((*it)->obj, cdata, callback)) return; } } } void SaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const { SaPCollisionManager* other_manager = static_cast(other_manager_); if((size() == 0) || (other_manager->size() == 0)) return; if(this == other_manager) { distance(cdata, callback); return; } FCL_REAL min_dist = std::numeric_limits::max(); if(this->size() < other_manager->size()) { for(std::list::const_iterator it = AABB_arr.begin(), end = AABB_arr.end(); it != end; ++it) { if(other_manager->distance_((*it)->obj, cdata, callback, min_dist)) return; } } else { for(std::list::const_iterator it = other_manager->AABB_arr.begin(), end = other_manager->AABB_arr.end(); it != end; ++it) { if(distance_((*it)->obj, cdata, callback, min_dist)) return; } } } bool SaPCollisionManager::empty() const { return AABB_arr.size(); } } fcl-0.5.0/src/broadphase/broadphase_bruteforce.cpp000066400000000000000000000135421274356571000222320ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/broadphase/broadphase_bruteforce.h" #include #include namespace fcl { void NaiveCollisionManager::registerObjects(const std::vector& other_objs) { std::copy(other_objs.begin(), other_objs.end(), std::back_inserter(objs)); } void NaiveCollisionManager::unregisterObject(CollisionObject* obj) { objs.remove(obj); } void NaiveCollisionManager::registerObject(CollisionObject* obj) { objs.push_back(obj); } void NaiveCollisionManager::setup() { } void NaiveCollisionManager::update() { } void NaiveCollisionManager::clear() { objs.clear(); } void NaiveCollisionManager::getObjects(std::vector& objs_) const { objs_.resize(objs.size()); std::copy(objs.begin(), objs.end(), objs_.begin()); } void NaiveCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const { if(size() == 0) return; for(std::list::const_iterator it = objs.begin(), end = objs.end(); it != end; ++it) { if(callback(obj, *it, cdata)) return; } } void NaiveCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const { if(size() == 0) return; FCL_REAL min_dist = std::numeric_limits::max(); for(std::list::const_iterator it = objs.begin(), end = objs.end(); it != end; ++it) { if(obj->getAABB().distance((*it)->getAABB()) < min_dist) { if(callback(obj, *it, cdata, min_dist)) return; } } } void NaiveCollisionManager::collide(void* cdata, CollisionCallBack callback) const { if(size() == 0) return; for(std::list::const_iterator it1 = objs.begin(), end = objs.end(); it1 != end; ++it1) { std::list::const_iterator it2 = it1; it2++; for(; it2 != end; ++it2) { if((*it1)->getAABB().overlap((*it2)->getAABB())) if(callback(*it1, *it2, cdata)) return; } } } void NaiveCollisionManager::distance(void* cdata, DistanceCallBack callback) const { if(size() == 0) return; FCL_REAL min_dist = std::numeric_limits::max(); for(std::list::const_iterator it1 = objs.begin(), end = objs.end(); it1 != end; ++it1) { std::list::const_iterator it2 = it1; it2++; for(; it2 != end; ++it2) { if((*it1)->getAABB().distance((*it2)->getAABB()) < min_dist) { if(callback(*it1, *it2, cdata, min_dist)) return; } } } } void NaiveCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const { NaiveCollisionManager* other_manager = static_cast(other_manager_); if((size() == 0) || (other_manager->size() == 0)) return; if(this == other_manager) { collide(cdata, callback); return; } for(std::list::const_iterator it1 = objs.begin(), end1 = objs.end(); it1 != end1; ++it1) { for(std::list::const_iterator it2 = other_manager->objs.begin(), end2 = other_manager->objs.end(); it2 != end2; ++it2) { if((*it1)->getAABB().overlap((*it2)->getAABB())) if(callback((*it1), (*it2), cdata)) return; } } } void NaiveCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const { NaiveCollisionManager* other_manager = static_cast(other_manager_); if((size() == 0) || (other_manager->size() == 0)) return; if(this == other_manager) { distance(cdata, callback); return; } FCL_REAL min_dist = std::numeric_limits::max(); for(std::list::const_iterator it1 = objs.begin(), end1 = objs.end(); it1 != end1; ++it1) { for(std::list::const_iterator it2 = other_manager->objs.begin(), end2 = other_manager->objs.end(); it2 != end2; ++it2) { if((*it1)->getAABB().distance((*it2)->getAABB()) < min_dist) { if(callback(*it1, *it2, cdata, min_dist)) return; } } } } bool NaiveCollisionManager::empty() const { return objs.empty(); } } fcl-0.5.0/src/broadphase/broadphase_dynamic_AABB_tree.cpp000066400000000000000000000602161274356571000233020ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/broadphase/broadphase_dynamic_AABB_tree.h" #if FCL_HAVE_OCTOMAP #include "fcl/octree.h" #endif namespace fcl { namespace details { namespace dynamic_AABB_tree { #if FCL_HAVE_OCTOMAP bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, CollisionCallBack callback) { if(!root2) { if(root1->isLeaf()) { CollisionObject* obj1 = static_cast(root1->data); if(!obj1->isFree()) { OBB obb1, obb2; convertBV(root1->bv, Transform3f(), obb1); convertBV(root2_bv, tf2, obb2); if(obb1.overlap(obb2)) { Box* box = new Box(); Transform3f box_tf; constructBox(root2_bv, tf2, *box, box_tf); box->cost_density = tree2->getDefaultOccupancy(); CollisionObject obj2(std::shared_ptr(box), box_tf); return callback(obj1, &obj2, cdata); } } } else { if(collisionRecurse_(root1->children[0], tree2, NULL, root2_bv, tf2, cdata, callback)) return true; if(collisionRecurse_(root1->children[1], tree2, NULL, root2_bv, tf2, cdata, callback)) return true; } return false; } else if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) { CollisionObject* obj1 = static_cast(root1->data); if(!tree2->isNodeFree(root2) && !obj1->isFree()) { OBB obb1, obb2; convertBV(root1->bv, Transform3f(), obb1); convertBV(root2_bv, tf2, obb2); if(obb1.overlap(obb2)) { Box* box = new Box(); Transform3f box_tf; constructBox(root2_bv, tf2, *box, box_tf); box->cost_density = root2->getOccupancy(); box->threshold_occupied = tree2->getOccupancyThres(); CollisionObject obj2(std::shared_ptr(box), box_tf); return callback(obj1, &obj2, cdata); } else return false; } else return false; } OBB obb1, obb2; convertBV(root1->bv, Transform3f(), obb1); convertBV(root2_bv, tf2, obb2); if(tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false; if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { if(collisionRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback)) return true; if(collisionRecurse_(root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback)) return true; } else { for(unsigned int i = 0; i < 8; ++i) { if(tree2->nodeChildExists(root2, i)) { const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(root2_bv, i, child_bv); if(collisionRecurse_(root1, tree2, child, child_bv, tf2, cdata, callback)) return true; } else { AABB child_bv; computeChildBV(root2_bv, i, child_bv); if(collisionRecurse_(root1, tree2, NULL, child_bv, tf2, cdata, callback)) return true; } } } return false; } bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Vec3f& tf2, void* cdata, CollisionCallBack callback) { if(!root2) { if(root1->isLeaf()) { CollisionObject* obj1 = static_cast(root1->data); if(!obj1->isFree()) { const AABB& root2_bv_t = translate(root2_bv, tf2); if(root1->bv.overlap(root2_bv_t)) { Box* box = new Box(); Transform3f box_tf; constructBox(root2_bv, tf2, *box, box_tf); box->cost_density = tree2->getOccupancyThres(); // thresholds are 0, 1, so uncertain CollisionObject obj2(std::shared_ptr(box), box_tf); return callback(obj1, &obj2, cdata); } } } else { if(collisionRecurse_(root1->children[0], tree2, NULL, root2_bv, tf2, cdata, callback)) return true; if(collisionRecurse_(root1->children[1], tree2, NULL, root2_bv, tf2, cdata, callback)) return true; } return false; } else if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) { CollisionObject* obj1 = static_cast(root1->data); if(!tree2->isNodeFree(root2) && !obj1->isFree()) { const AABB& root2_bv_t = translate(root2_bv, tf2); if(root1->bv.overlap(root2_bv_t)) { Box* box = new Box(); Transform3f box_tf; constructBox(root2_bv, tf2, *box, box_tf); box->cost_density = root2->getOccupancy(); box->threshold_occupied = tree2->getOccupancyThres(); CollisionObject obj2(std::shared_ptr(box), box_tf); return callback(obj1, &obj2, cdata); } else return false; } else return false; } const AABB& root2_bv_t = translate(root2_bv, tf2); if(tree2->isNodeFree(root2) || !root1->bv.overlap(root2_bv_t)) return false; if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { if(collisionRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback)) return true; if(collisionRecurse_(root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback)) return true; } else { for(unsigned int i = 0; i < 8; ++i) { if(tree2->nodeChildExists(root2, i)) { const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(root2_bv, i, child_bv); if(collisionRecurse_(root1, tree2, child, child_bv, tf2, cdata, callback)) return true; } else { AABB child_bv; computeChildBV(root2_bv, i, child_bv); if(collisionRecurse_(root1, tree2, NULL, child_bv, tf2, cdata, callback)) return true; } } } return false; } bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) { if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) { if(tree2->isNodeOccupied(root2)) { Box* box = new Box(); Transform3f box_tf; constructBox(root2_bv, tf2, *box, box_tf); CollisionObject obj(std::shared_ptr(box), box_tf); return callback(static_cast(root1->data), &obj, cdata, min_dist); } else return false; } if(!tree2->isNodeOccupied(root2)) return false; if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { AABB aabb2; convertBV(root2_bv, tf2, aabb2); FCL_REAL d1 = aabb2.distance(root1->children[0]->bv); FCL_REAL d2 = aabb2.distance(root1->children[1]->bv); if(d2 < d1) { if(d2 < min_dist) { if(distanceRecurse_(root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist)) return true; } if(d1 < min_dist) { if(distanceRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist)) return true; } } else { if(d1 < min_dist) { if(distanceRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist)) return true; } if(d2 < min_dist) { if(distanceRecurse_(root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist)) return true; } } } else { for(unsigned int i = 0; i < 8; ++i) { if(tree2->nodeChildExists(root2, i)) { const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(root2_bv, i, child_bv); AABB aabb2; convertBV(child_bv, tf2, aabb2); FCL_REAL d = root1->bv.distance(aabb2); if(d < min_dist) { if(distanceRecurse_(root1, tree2, child, child_bv, tf2, cdata, callback, min_dist)) return true; } } } } return false; } bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, CollisionCallBack callback) { if(tf2.getQuatRotation().isIdentity()) return collisionRecurse_(root1, tree2, root2, root2_bv, tf2.getTranslation(), cdata, callback); else // has rotation return collisionRecurse_(root1, tree2, root2, root2_bv, tf2, cdata, callback); } bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Vec3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) { if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) { if(tree2->isNodeOccupied(root2)) { Box* box = new Box(); Transform3f box_tf; constructBox(root2_bv, tf2, *box, box_tf); CollisionObject obj(std::shared_ptr(box), box_tf); return callback(static_cast(root1->data), &obj, cdata, min_dist); } else return false; } if(!tree2->isNodeOccupied(root2)) return false; if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { const AABB& aabb2 = translate(root2_bv, tf2); FCL_REAL d1 = aabb2.distance(root1->children[0]->bv); FCL_REAL d2 = aabb2.distance(root1->children[1]->bv); if(d2 < d1) { if(d2 < min_dist) { if(distanceRecurse_(root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist)) return true; } if(d1 < min_dist) { if(distanceRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist)) return true; } } else { if(d1 < min_dist) { if(distanceRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist)) return true; } if(d2 < min_dist) { if(distanceRecurse_(root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist)) return true; } } } else { for(unsigned int i = 0; i < 8; ++i) { if(tree2->nodeChildExists(root2, i)) { const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(root2_bv, i, child_bv); const AABB& aabb2 = translate(child_bv, tf2); FCL_REAL d = root1->bv.distance(aabb2); if(d < min_dist) { if(distanceRecurse_(root1, tree2, child, child_bv, tf2, cdata, callback, min_dist)) return true; } } } } return false; } bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) { if(tf2.getQuatRotation().isIdentity()) return distanceRecurse_(root1, tree2, root2, root2_bv, tf2.getTranslation(), cdata, callback, min_dist); else return distanceRecurse_(root1, tree2, root2, root2_bv, tf2, cdata, callback, min_dist); } #endif bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, DynamicAABBTreeCollisionManager::DynamicAABBNode* root2, void* cdata, CollisionCallBack callback) { if(root1->isLeaf() && root2->isLeaf()) { if(!root1->bv.overlap(root2->bv)) return false; return callback(static_cast(root1->data), static_cast(root2->data), cdata); } if(!root1->bv.overlap(root2->bv)) return false; if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) { if(collisionRecurse(root1->children[0], root2, cdata, callback)) return true; if(collisionRecurse(root1->children[1], root2, cdata, callback)) return true; } else { if(collisionRecurse(root1, root2->children[0], cdata, callback)) return true; if(collisionRecurse(root1, root2->children[1], cdata, callback)) return true; } return false; } bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, CollisionObject* query, void* cdata, CollisionCallBack callback) { if(root->isLeaf()) { if(!root->bv.overlap(query->getAABB())) return false; return callback(static_cast(root->data), query, cdata); } if(!root->bv.overlap(query->getAABB())) return false; int select_res = select(query->getAABB(), *(root->children[0]), *(root->children[1])); if(collisionRecurse(root->children[select_res], query, cdata, callback)) return true; if(collisionRecurse(root->children[1-select_res], query, cdata, callback)) return true; return false; } bool selfCollisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, void* cdata, CollisionCallBack callback) { if(root->isLeaf()) return false; if(selfCollisionRecurse(root->children[0], cdata, callback)) return true; if(selfCollisionRecurse(root->children[1], cdata, callback)) return true; if(collisionRecurse(root->children[0], root->children[1], cdata, callback)) return true; return false; } bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, DynamicAABBTreeCollisionManager::DynamicAABBNode* root2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) { if(root1->isLeaf() && root2->isLeaf()) { CollisionObject* root1_obj = static_cast(root1->data); CollisionObject* root2_obj = static_cast(root2->data); return callback(root1_obj, root2_obj, cdata, min_dist); } if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) { FCL_REAL d1 = root2->bv.distance(root1->children[0]->bv); FCL_REAL d2 = root2->bv.distance(root1->children[1]->bv); if(d2 < d1) { if(d2 < min_dist) { if(distanceRecurse(root1->children[1], root2, cdata, callback, min_dist)) return true; } if(d1 < min_dist) { if(distanceRecurse(root1->children[0], root2, cdata, callback, min_dist)) return true; } } else { if(d1 < min_dist) { if(distanceRecurse(root1->children[0], root2, cdata, callback, min_dist)) return true; } if(d2 < min_dist) { if(distanceRecurse(root1->children[1], root2, cdata, callback, min_dist)) return true; } } } else { FCL_REAL d1 = root1->bv.distance(root2->children[0]->bv); FCL_REAL d2 = root1->bv.distance(root2->children[1]->bv); if(d2 < d1) { if(d2 < min_dist) { if(distanceRecurse(root1, root2->children[1], cdata, callback, min_dist)) return true; } if(d1 < min_dist) { if(distanceRecurse(root1, root2->children[0], cdata, callback, min_dist)) return true; } } else { if(d1 < min_dist) { if(distanceRecurse(root1, root2->children[0], cdata, callback, min_dist)) return true; } if(d2 < min_dist) { if(distanceRecurse(root1, root2->children[1], cdata, callback, min_dist)) return true; } } } return false; } bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, CollisionObject* query, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) { if(root->isLeaf()) { CollisionObject* root_obj = static_cast(root->data); return callback(root_obj, query, cdata, min_dist); } FCL_REAL d1 = query->getAABB().distance(root->children[0]->bv); FCL_REAL d2 = query->getAABB().distance(root->children[1]->bv); if(d2 < d1) { if(d2 < min_dist) { if(distanceRecurse(root->children[1], query, cdata, callback, min_dist)) return true; } if(d1 < min_dist) { if(distanceRecurse(root->children[0], query, cdata, callback, min_dist)) return true; } } else { if(d1 < min_dist) { if(distanceRecurse(root->children[0], query, cdata, callback, min_dist)) return true; } if(d2 < min_dist) { if(distanceRecurse(root->children[1], query, cdata, callback, min_dist)) return true; } } return false; } bool selfDistanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) { if(root->isLeaf()) return false; if(selfDistanceRecurse(root->children[0], cdata, callback, min_dist)) return true; if(selfDistanceRecurse(root->children[1], cdata, callback, min_dist)) return true; if(distanceRecurse(root->children[0], root->children[1], cdata, callback, min_dist)) return true; return false; } } // dynamic_AABB_tree } // details void DynamicAABBTreeCollisionManager::registerObjects(const std::vector& other_objs) { if(other_objs.empty()) return; if(size() > 0) { BroadPhaseCollisionManager::registerObjects(other_objs); } else { std::vector leaves(other_objs.size()); table.rehash(other_objs.size()); for(size_t i = 0, size = other_objs.size(); i < size; ++i) { DynamicAABBNode* node = new DynamicAABBNode; // node will be managed by the dtree node->bv = other_objs[i]->getAABB(); node->parent = NULL; node->children[1] = NULL; node->data = other_objs[i]; table[other_objs[i]] = node; leaves[i] = node; } dtree.init(leaves, tree_init_level); setup_ = true; } } void DynamicAABBTreeCollisionManager::registerObject(CollisionObject* obj) { DynamicAABBNode* node = dtree.insert(obj->getAABB(), obj); table[obj] = node; } void DynamicAABBTreeCollisionManager::unregisterObject(CollisionObject* obj) { DynamicAABBNode* node = table[obj]; table.erase(obj); dtree.remove(node); } void DynamicAABBTreeCollisionManager::setup() { if(!setup_) { int num = dtree.size(); if(num == 0) { setup_ = true; return; } int height = dtree.getMaxHeight(); if(height - std::log((FCL_REAL)num) / std::log(2.0) < max_tree_nonbalanced_level) dtree.balanceIncremental(tree_incremental_balance_pass); else dtree.balanceTopdown(); setup_ = true; } } void DynamicAABBTreeCollisionManager::update() { for(DynamicAABBTable::const_iterator it = table.begin(); it != table.end(); ++it) { CollisionObject* obj = it->first; DynamicAABBNode* node = it->second; node->bv = obj->getAABB(); } dtree.refit(); setup_ = false; setup(); } void DynamicAABBTreeCollisionManager::update_(CollisionObject* updated_obj) { DynamicAABBTable::const_iterator it = table.find(updated_obj); if(it != table.end()) { DynamicAABBNode* node = it->second; if(!node->bv.equal(updated_obj->getAABB())) dtree.update(node, updated_obj->getAABB()); } setup_ = false; } void DynamicAABBTreeCollisionManager::update(CollisionObject* updated_obj) { update_(updated_obj); setup(); } void DynamicAABBTreeCollisionManager::update(const std::vector& updated_objs) { for(size_t i = 0, size = updated_objs.size(); i < size; ++i) update_(updated_objs[i]); setup(); } void DynamicAABBTreeCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const { if(size() == 0) return; switch(obj->collisionGeometry()->getNodeType()) { #if FCL_HAVE_OCTOMAP case GEOM_OCTREE: { if(!octree_as_geometry_collide) { const OcTree* octree = static_cast(obj->collisionGeometry().get()); details::dynamic_AABB_tree::collisionRecurse(dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback); } else details::dynamic_AABB_tree::collisionRecurse(dtree.getRoot(), obj, cdata, callback); } break; #endif default: details::dynamic_AABB_tree::collisionRecurse(dtree.getRoot(), obj, cdata, callback); } } void DynamicAABBTreeCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const { if(size() == 0) return; FCL_REAL min_dist = std::numeric_limits::max(); switch(obj->collisionGeometry()->getNodeType()) { #if FCL_HAVE_OCTOMAP case GEOM_OCTREE: { if(!octree_as_geometry_distance) { const OcTree* octree = static_cast(obj->collisionGeometry().get()); details::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback, min_dist); } else details::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), obj, cdata, callback, min_dist); } break; #endif default: details::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), obj, cdata, callback, min_dist); } } void DynamicAABBTreeCollisionManager::collide(void* cdata, CollisionCallBack callback) const { if(size() == 0) return; details::dynamic_AABB_tree::selfCollisionRecurse(dtree.getRoot(), cdata, callback); } void DynamicAABBTreeCollisionManager::distance(void* cdata, DistanceCallBack callback) const { if(size() == 0) return; FCL_REAL min_dist = std::numeric_limits::max(); details::dynamic_AABB_tree::selfDistanceRecurse(dtree.getRoot(), cdata, callback, min_dist); } void DynamicAABBTreeCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const { DynamicAABBTreeCollisionManager* other_manager = static_cast(other_manager_); if((size() == 0) || (other_manager->size() == 0)) return; details::dynamic_AABB_tree::collisionRecurse(dtree.getRoot(), other_manager->dtree.getRoot(), cdata, callback); } void DynamicAABBTreeCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const { DynamicAABBTreeCollisionManager* other_manager = static_cast(other_manager_); if((size() == 0) || (other_manager->size() == 0)) return; FCL_REAL min_dist = std::numeric_limits::max(); details::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), other_manager->dtree.getRoot(), cdata, callback, min_dist); } } fcl-0.5.0/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp000066400000000000000000000651501274356571000245020ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/broadphase/broadphase_dynamic_AABB_tree_array.h" #if FCL_HAVE_OCTOMAP #include "fcl/octree.h" #endif namespace fcl { namespace details { namespace dynamic_AABB_tree_array { #if FCL_HAVE_OCTOMAP bool collisionRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, CollisionCallBack callback) { DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root1 = nodes1 + root1_id; if(!root2) { if(root1->isLeaf()) { CollisionObject* obj1 = static_cast(root1->data); if(!obj1->isFree()) { OBB obb1, obb2; convertBV(root1->bv, Transform3f(), obb1); convertBV(root2_bv, tf2, obb2); if(obb1.overlap(obb2)) { Box* box = new Box(); Transform3f box_tf; constructBox(root2_bv, tf2, *box, box_tf); box->cost_density = tree2->getDefaultOccupancy(); CollisionObject obj2(std::shared_ptr(box), box_tf); return callback(obj1, &obj2, cdata); } } } else { if(collisionRecurse_(nodes1, root1->children[0], tree2, NULL, root2_bv, tf2, cdata, callback)) return true; if(collisionRecurse_(nodes1, root1->children[1], tree2, NULL, root2_bv, tf2, cdata, callback)) return true; } return false; } else if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) { CollisionObject* obj1 = static_cast(root1->data); if(!tree2->isNodeFree(root2) && !obj1->isFree()) { OBB obb1, obb2; convertBV(root1->bv, Transform3f(), obb1); convertBV(root2_bv, tf2, obb2); if(obb1.overlap(obb2)) { Box* box = new Box(); Transform3f box_tf; constructBox(root2_bv, tf2, *box, box_tf); box->cost_density = root2->getOccupancy(); box->threshold_occupied = tree2->getOccupancyThres(); CollisionObject obj2(std::shared_ptr(box), box_tf); return callback(obj1, &obj2, cdata); } else return false; } else return false; } OBB obb1, obb2; convertBV(root1->bv, Transform3f(), obb1); convertBV(root2_bv, tf2, obb2); if(tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false; if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { if(collisionRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback)) return true; if(collisionRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback)) return true; } else { for(unsigned int i = 0; i < 8; ++i) { if(tree2->nodeChildExists(root2, i)) { const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(root2_bv, i, child_bv); if(collisionRecurse_(nodes1, root1_id, tree2, child, child_bv, tf2, cdata, callback)) return true; } else { AABB child_bv; computeChildBV(root2_bv, i, child_bv); if(collisionRecurse_(nodes1, root1_id, tree2, NULL, child_bv, tf2, cdata, callback)) return true; } } } return false; } bool collisionRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Vec3f& tf2, void* cdata, CollisionCallBack callback) { DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root1 = nodes1 + root1_id; if(!root2) { if(root1->isLeaf()) { CollisionObject* obj1 = static_cast(root1->data); if(!obj1->isFree()) { const AABB& root_bv_t = translate(root2_bv, tf2); if(root1->bv.overlap(root_bv_t)) { Box* box = new Box(); Transform3f box_tf; constructBox(root2_bv, tf2, *box, box_tf); box->cost_density = tree2->getDefaultOccupancy(); CollisionObject obj2(std::shared_ptr(box), box_tf); return callback(obj1, &obj2, cdata); } } } else { if(collisionRecurse_(nodes1, root1->children[0], tree2, NULL, root2_bv, tf2, cdata, callback)) return true; if(collisionRecurse_(nodes1, root1->children[1], tree2, NULL, root2_bv, tf2, cdata, callback)) return true; } return false; } else if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) { CollisionObject* obj1 = static_cast(root1->data); if(!tree2->isNodeFree(root2) && !obj1->isFree()) { const AABB& root_bv_t = translate(root2_bv, tf2); if(root1->bv.overlap(root_bv_t)) { Box* box = new Box(); Transform3f box_tf; constructBox(root2_bv, tf2, *box, box_tf); box->cost_density = root2->getOccupancy(); box->threshold_occupied = tree2->getOccupancyThres(); CollisionObject obj2(std::shared_ptr(box), box_tf); return callback(obj1, &obj2, cdata); } else return false; } else return false; } const AABB& root_bv_t = translate(root2_bv, tf2); if(tree2->isNodeFree(root2) || !root1->bv.overlap(root_bv_t)) return false; if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { if(collisionRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback)) return true; if(collisionRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback)) return true; } else { for(unsigned int i = 0; i < 8; ++i) { if(tree2->nodeChildExists(root2, i)) { const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(root2_bv, i, child_bv); if(collisionRecurse_(nodes1, root1_id, tree2, child, child_bv, tf2, cdata, callback)) return true; } else { AABB child_bv; computeChildBV(root2_bv, i, child_bv); if(collisionRecurse_(nodes1, root1_id, tree2, NULL, child_bv, tf2, cdata, callback)) return true; } } } return false; } bool distanceRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) { DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root1 = nodes1 + root1_id; if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) { if(tree2->isNodeOccupied(root2)) { Box* box = new Box(); Transform3f box_tf; constructBox(root2_bv, tf2, *box, box_tf); CollisionObject obj(std::shared_ptr(box), box_tf); return callback(static_cast(root1->data), &obj, cdata, min_dist); } else return false; } if(!tree2->isNodeOccupied(root2)) return false; if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { AABB aabb2; convertBV(root2_bv, tf2, aabb2); FCL_REAL d1 = aabb2.distance((nodes1 + root1->children[0])->bv); FCL_REAL d2 = aabb2.distance((nodes1 + root1->children[1])->bv); if(d2 < d1) { if(d2 < min_dist) { if(distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist)) return true; } if(d1 < min_dist) { if(distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist)) return true; } } else { if(d1 < min_dist) { if(distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist)) return true; } if(d2 < min_dist) { if(distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist)) return true; } } } else { for(unsigned int i = 0; i < 8; ++i) { if(tree2->nodeChildExists(root2, i)) { const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(root2_bv, i, child_bv); AABB aabb2; convertBV(child_bv, tf2, aabb2); FCL_REAL d = root1->bv.distance(aabb2); if(d < min_dist) { if(distanceRecurse_(nodes1, root1_id, tree2, child, child_bv, tf2, cdata, callback, min_dist)) return true; } } } } return false; } bool distanceRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Vec3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) { DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root1 = nodes1 + root1_id; if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) { if(tree2->isNodeOccupied(root2)) { Box* box = new Box(); Transform3f box_tf; constructBox(root2_bv, tf2, *box, box_tf); CollisionObject obj(std::shared_ptr(box), box_tf); return callback(static_cast(root1->data), &obj, cdata, min_dist); } else return false; } if(!tree2->isNodeOccupied(root2)) return false; if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { const AABB& aabb2 = translate(root2_bv, tf2); FCL_REAL d1 = aabb2.distance((nodes1 + root1->children[0])->bv); FCL_REAL d2 = aabb2.distance((nodes1 + root1->children[1])->bv); if(d2 < d1) { if(d2 < min_dist) { if(distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist)) return true; } if(d1 < min_dist) { if(distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist)) return true; } } else { if(d1 < min_dist) { if(distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist)) return true; } if(d2 < min_dist) { if(distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist)) return true; } } } else { for(unsigned int i = 0; i < 8; ++i) { if(tree2->nodeChildExists(root2, i)) { const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(root2_bv, i, child_bv); const AABB& aabb2 = translate(child_bv, tf2); FCL_REAL d = root1->bv.distance(aabb2); if(d < min_dist) { if(distanceRecurse_(nodes1, root1_id, tree2, child, child_bv, tf2, cdata, callback, min_dist)) return true; } } } } return false; } #endif bool collisionRecurse(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes2, size_t root2_id, void* cdata, CollisionCallBack callback) { DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root1 = nodes1 + root1_id; DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root2 = nodes2 + root2_id; if(root1->isLeaf() && root2->isLeaf()) { if(!root1->bv.overlap(root2->bv)) return false; return callback(static_cast(root1->data), static_cast(root2->data), cdata); } if(!root1->bv.overlap(root2->bv)) return false; if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) { if(collisionRecurse(nodes1, root1->children[0], nodes2, root2_id, cdata, callback)) return true; if(collisionRecurse(nodes1, root1->children[1], nodes2, root2_id, cdata, callback)) return true; } else { if(collisionRecurse(nodes1, root1_id, nodes2, root2->children[0], cdata, callback)) return true; if(collisionRecurse(nodes1, root1_id, nodes2, root2->children[1], cdata, callback)) return true; } return false; } bool collisionRecurse(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes, size_t root_id, CollisionObject* query, void* cdata, CollisionCallBack callback) { DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root = nodes + root_id; if(root->isLeaf()) { if(!root->bv.overlap(query->getAABB())) return false; return callback(static_cast(root->data), query, cdata); } if(!root->bv.overlap(query->getAABB())) return false; int select_res = implementation_array::select(query->getAABB(), root->children[0], root->children[1], nodes); if(collisionRecurse(nodes, root->children[select_res], query, cdata, callback)) return true; if(collisionRecurse(nodes, root->children[1-select_res], query, cdata, callback)) return true; return false; } bool selfCollisionRecurse(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes, size_t root_id, void* cdata, CollisionCallBack callback) { DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root = nodes + root_id; if(root->isLeaf()) return false; if(selfCollisionRecurse(nodes, root->children[0], cdata, callback)) return true; if(selfCollisionRecurse(nodes, root->children[1], cdata, callback)) return true; if(collisionRecurse(nodes, root->children[0], nodes, root->children[1], cdata, callback)) return true; return false; } bool distanceRecurse(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes2, size_t root2_id, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) { DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root1 = nodes1 + root1_id; DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root2 = nodes2 + root2_id; if(root1->isLeaf() && root2->isLeaf()) { CollisionObject* root1_obj = static_cast(root1->data); CollisionObject* root2_obj = static_cast(root2->data); return callback(root1_obj, root2_obj, cdata, min_dist); } if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) { FCL_REAL d1 = root2->bv.distance((nodes1 + root1->children[0])->bv); FCL_REAL d2 = root2->bv.distance((nodes1 + root1->children[1])->bv); if(d2 < d1) { if(d2 < min_dist) { if(distanceRecurse(nodes1, root1->children[1], nodes2, root2_id, cdata, callback, min_dist)) return true; } if(d1 < min_dist) { if(distanceRecurse(nodes1, root1->children[0], nodes2, root2_id, cdata, callback, min_dist)) return true; } } else { if(d1 < min_dist) { if(distanceRecurse(nodes1, root1->children[0], nodes2, root2_id, cdata, callback, min_dist)) return true; } if(d2 < min_dist) { if(distanceRecurse(nodes1, root1->children[1], nodes2, root2_id, cdata, callback, min_dist)) return true; } } } else { FCL_REAL d1 = root1->bv.distance((nodes2 + root2->children[0])->bv); FCL_REAL d2 = root1->bv.distance((nodes2 + root2->children[1])->bv); if(d2 < d1) { if(d2 < min_dist) { if(distanceRecurse(nodes1, root1_id, nodes2, root2->children[1], cdata, callback, min_dist)) return true; } if(d1 < min_dist) { if(distanceRecurse(nodes1, root1_id, nodes2, root2->children[0], cdata, callback, min_dist)) return true; } } else { if(d1 < min_dist) { if(distanceRecurse(nodes1, root1_id, nodes2, root2->children[0], cdata, callback, min_dist)) return true; } if(d2 < min_dist) { if(distanceRecurse(nodes1, root1_id, nodes2, root2->children[1], cdata, callback, min_dist)) return true; } } } return false; } bool distanceRecurse(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes, size_t root_id, CollisionObject* query, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) { DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root = nodes + root_id; if(root->isLeaf()) { CollisionObject* root_obj = static_cast(root->data); return callback(root_obj, query, cdata, min_dist); } FCL_REAL d1 = query->getAABB().distance((nodes + root->children[0])->bv); FCL_REAL d2 = query->getAABB().distance((nodes + root->children[1])->bv); if(d2 < d1) { if(d2 < min_dist) { if(distanceRecurse(nodes, root->children[1], query, cdata, callback, min_dist)) return true; } if(d1 < min_dist) { if(distanceRecurse(nodes, root->children[0], query, cdata, callback, min_dist)) return true; } } else { if(d1 < min_dist) { if(distanceRecurse(nodes, root->children[0], query, cdata, callback, min_dist)) return true; } if(d2 < min_dist) { if(distanceRecurse(nodes, root->children[1], query, cdata, callback, min_dist)) return true; } } return false; } bool selfDistanceRecurse(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes, size_t root_id, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) { DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root = nodes + root_id; if(root->isLeaf()) return false; if(selfDistanceRecurse(nodes, root->children[0], cdata, callback, min_dist)) return true; if(selfDistanceRecurse(nodes, root->children[1], cdata, callback, min_dist)) return true; if(distanceRecurse(nodes, root->children[0], nodes, root->children[1], cdata, callback, min_dist)) return true; return false; } #if FCL_HAVE_OCTOMAP bool collisionRecurse(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, CollisionCallBack callback) { if(tf2.getQuatRotation().isIdentity()) return collisionRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2.getTranslation(), cdata, callback); else return collisionRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2, cdata, callback); } bool distanceRecurse(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) { if(tf2.getQuatRotation().isIdentity()) return distanceRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2.getTranslation(), cdata, callback, min_dist); else return distanceRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2, cdata, callback, min_dist); } #endif } // dynamic_AABB_tree_array } // details void DynamicAABBTreeCollisionManager_Array::registerObjects(const std::vector& other_objs) { if(other_objs.empty()) return; if(size() > 0) { BroadPhaseCollisionManager::registerObjects(other_objs); } else { DynamicAABBNode* leaves = new DynamicAABBNode[other_objs.size()]; table.rehash(other_objs.size()); for(size_t i = 0, size = other_objs.size(); i < size; ++i) { leaves[i].bv = other_objs[i]->getAABB(); leaves[i].parent = dtree.NULL_NODE; leaves[i].children[1] = dtree.NULL_NODE; leaves[i].data = other_objs[i]; table[other_objs[i]] = i; } int n_leaves = other_objs.size(); dtree.init(leaves, n_leaves, tree_init_level); setup_ = true; } } void DynamicAABBTreeCollisionManager_Array::registerObject(CollisionObject* obj) { size_t node = dtree.insert(obj->getAABB(), obj); table[obj] = node; } void DynamicAABBTreeCollisionManager_Array::unregisterObject(CollisionObject* obj) { size_t node = table[obj]; table.erase(obj); dtree.remove(node); } void DynamicAABBTreeCollisionManager_Array::setup() { if(!setup_) { int num = dtree.size(); if(num == 0) { setup_ = true; return; } int height = dtree.getMaxHeight(); if(height - std::log((FCL_REAL)num) / std::log(2.0) < max_tree_nonbalanced_level) dtree.balanceIncremental(tree_incremental_balance_pass); else dtree.balanceTopdown(); setup_ = true; } } void DynamicAABBTreeCollisionManager_Array::update() { for(DynamicAABBTable::const_iterator it = table.begin(), end = table.end(); it != end; ++it) { CollisionObject* obj = it->first; size_t node = it->second; dtree.getNodes()[node].bv = obj->getAABB(); } dtree.refit(); setup_ = false; setup(); } void DynamicAABBTreeCollisionManager_Array::update_(CollisionObject* updated_obj) { DynamicAABBTable::const_iterator it = table.find(updated_obj); if(it != table.end()) { size_t node = it->second; if(!dtree.getNodes()[node].bv.equal(updated_obj->getAABB())) dtree.update(node, updated_obj->getAABB()); } setup_ = false; } void DynamicAABBTreeCollisionManager_Array::update(CollisionObject* updated_obj) { update_(updated_obj); setup(); } void DynamicAABBTreeCollisionManager_Array::update(const std::vector& updated_objs) { for(size_t i = 0, size = updated_objs.size(); i < size; ++i) update_(updated_objs[i]); setup(); } void DynamicAABBTreeCollisionManager_Array::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const { if(size() == 0) return; switch(obj->collisionGeometry()->getNodeType()) { #if FCL_HAVE_OCTOMAP case GEOM_OCTREE: { if(!octree_as_geometry_collide) { const OcTree* octree = static_cast(obj->collisionGeometry().get()); details::dynamic_AABB_tree_array::collisionRecurse(dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback); } else details::dynamic_AABB_tree_array::collisionRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback); } break; #endif default: details::dynamic_AABB_tree_array::collisionRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback); } } void DynamicAABBTreeCollisionManager_Array::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const { if(size() == 0) return; FCL_REAL min_dist = std::numeric_limits::max(); switch(obj->collisionGeometry()->getNodeType()) { #if FCL_HAVE_OCTOMAP case GEOM_OCTREE: { if(!octree_as_geometry_distance) { const OcTree* octree = static_cast(obj->collisionGeometry().get()); details::dynamic_AABB_tree_array::distanceRecurse(dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback, min_dist); } else details::dynamic_AABB_tree_array::distanceRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback, min_dist); } break; #endif default: details::dynamic_AABB_tree_array::distanceRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback, min_dist); } } void DynamicAABBTreeCollisionManager_Array::collide(void* cdata, CollisionCallBack callback) const { if(size() == 0) return; details::dynamic_AABB_tree_array::selfCollisionRecurse(dtree.getNodes(), dtree.getRoot(), cdata, callback); } void DynamicAABBTreeCollisionManager_Array::distance(void* cdata, DistanceCallBack callback) const { if(size() == 0) return; FCL_REAL min_dist = std::numeric_limits::max(); details::dynamic_AABB_tree_array::selfDistanceRecurse(dtree.getNodes(), dtree.getRoot(), cdata, callback, min_dist); } void DynamicAABBTreeCollisionManager_Array::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const { DynamicAABBTreeCollisionManager_Array* other_manager = static_cast(other_manager_); if((size() == 0) || (other_manager->size() == 0)) return; details::dynamic_AABB_tree_array::collisionRecurse(dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(), other_manager->dtree.getRoot(), cdata, callback); } void DynamicAABBTreeCollisionManager_Array::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const { DynamicAABBTreeCollisionManager_Array* other_manager = static_cast(other_manager_); if((size() == 0) || (other_manager->size() == 0)) return; FCL_REAL min_dist = std::numeric_limits::max(); details::dynamic_AABB_tree_array::distanceRecurse(dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(), other_manager->dtree.getRoot(), cdata, callback, min_dist); } } fcl-0.5.0/src/broadphase/broadphase_interval_tree.cpp000066400000000000000000000460021274356571000227320ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/broadphase/broadphase_interval_tree.h" #include #include #include namespace fcl { void IntervalTreeCollisionManager::unregisterObject(CollisionObject* obj) { // must sorted before setup(); EndPoint p; p.value = obj->getAABB().min_[0]; std::vector::iterator start1 = std::lower_bound(endpoints[0].begin(), endpoints[0].end(), p); p.value = obj->getAABB().max_[0]; std::vector::iterator end1 = std::upper_bound(start1, endpoints[0].end(), p); if(start1 < end1) { unsigned int start_id = start1 - endpoints[0].begin(); unsigned int end_id = end1 - endpoints[0].begin(); unsigned int cur_id = start_id; for(unsigned int i = start_id; i < end_id; ++i) { if(endpoints[0][i].obj != obj) { if(i == cur_id) cur_id++; else { endpoints[0][cur_id] = endpoints[0][i]; cur_id++; } } } if(cur_id < end_id) endpoints[0].resize(endpoints[0].size() - 2); } p.value = obj->getAABB().min_[1]; std::vector::iterator start2 = std::lower_bound(endpoints[1].begin(), endpoints[1].end(), p); p.value = obj->getAABB().max_[1]; std::vector::iterator end2 = std::upper_bound(start2, endpoints[1].end(), p); if(start2 < end2) { unsigned int start_id = start2 - endpoints[1].begin(); unsigned int end_id = end2 - endpoints[1].begin(); unsigned int cur_id = start_id; for(unsigned int i = start_id; i < end_id; ++i) { if(endpoints[1][i].obj != obj) { if(i == cur_id) cur_id++; else { endpoints[1][cur_id] = endpoints[1][i]; cur_id++; } } } if(cur_id < end_id) endpoints[1].resize(endpoints[1].size() - 2); } p.value = obj->getAABB().min_[2]; std::vector::iterator start3 = std::lower_bound(endpoints[2].begin(), endpoints[2].end(), p); p.value = obj->getAABB().max_[2]; std::vector::iterator end3 = std::upper_bound(start3, endpoints[2].end(), p); if(start3 < end3) { unsigned int start_id = start3 - endpoints[2].begin(); unsigned int end_id = end3 - endpoints[2].begin(); unsigned int cur_id = start_id; for(unsigned int i = start_id; i < end_id; ++i) { if(endpoints[2][i].obj != obj) { if(i == cur_id) cur_id++; else { endpoints[2][cur_id] = endpoints[2][i]; cur_id++; } } } if(cur_id < end_id) endpoints[2].resize(endpoints[2].size() - 2); } // update the interval tree if(obj_interval_maps[0].find(obj) != obj_interval_maps[0].end()) { SAPInterval* ivl1 = obj_interval_maps[0][obj]; SAPInterval* ivl2 = obj_interval_maps[1][obj]; SAPInterval* ivl3 = obj_interval_maps[2][obj]; interval_trees[0]->deleteNode(ivl1); interval_trees[1]->deleteNode(ivl2); interval_trees[2]->deleteNode(ivl3); delete ivl1; delete ivl2; delete ivl3; obj_interval_maps[0].erase(obj); obj_interval_maps[1].erase(obj); obj_interval_maps[2].erase(obj); } } void IntervalTreeCollisionManager::registerObject(CollisionObject* obj) { EndPoint p, q; p.obj = obj; q.obj = obj; p.minmax = 0; q.minmax = 1; p.value = obj->getAABB().min_[0]; q.value = obj->getAABB().max_[0]; endpoints[0].push_back(p); endpoints[0].push_back(q); p.value = obj->getAABB().min_[1]; q.value = obj->getAABB().max_[1]; endpoints[1].push_back(p); endpoints[1].push_back(q); p.value = obj->getAABB().min_[2]; q.value = obj->getAABB().max_[2]; endpoints[2].push_back(p); endpoints[2].push_back(q); setup_ = false; } void IntervalTreeCollisionManager::setup() { if(!setup_) { std::sort(endpoints[0].begin(), endpoints[0].end()); std::sort(endpoints[1].begin(), endpoints[1].end()); std::sort(endpoints[2].begin(), endpoints[2].end()); for(int i = 0; i < 3; ++i) delete interval_trees[i]; for(int i = 0; i < 3; ++i) interval_trees[i] = new IntervalTree; for(unsigned int i = 0, size = endpoints[0].size(); i < size; ++i) { EndPoint p = endpoints[0][i]; CollisionObject* obj = p.obj; if(p.minmax == 0) { SAPInterval* ivl1 = new SAPInterval(obj->getAABB().min_[0], obj->getAABB().max_[0], obj); SAPInterval* ivl2 = new SAPInterval(obj->getAABB().min_[1], obj->getAABB().max_[1], obj); SAPInterval* ivl3 = new SAPInterval(obj->getAABB().min_[2], obj->getAABB().max_[2], obj); interval_trees[0]->insert(ivl1); interval_trees[1]->insert(ivl2); interval_trees[2]->insert(ivl3); obj_interval_maps[0][obj] = ivl1; obj_interval_maps[1][obj] = ivl2; obj_interval_maps[2][obj] = ivl3; } } setup_ = true; } } void IntervalTreeCollisionManager::update() { setup_ = false; for(unsigned int i = 0, size = endpoints[0].size(); i < size; ++i) { if(endpoints[0][i].minmax == 0) endpoints[0][i].value = endpoints[0][i].obj->getAABB().min_[0]; else endpoints[0][i].value = endpoints[0][i].obj->getAABB().max_[0]; } for(unsigned int i = 0, size = endpoints[1].size(); i < size; ++i) { if(endpoints[1][i].minmax == 0) endpoints[1][i].value = endpoints[1][i].obj->getAABB().min_[1]; else endpoints[1][i].value = endpoints[1][i].obj->getAABB().max_[1]; } for(unsigned int i = 0, size = endpoints[2].size(); i < size; ++i) { if(endpoints[2][i].minmax == 0) endpoints[2][i].value = endpoints[2][i].obj->getAABB().min_[2]; else endpoints[2][i].value = endpoints[2][i].obj->getAABB().max_[2]; } setup(); } void IntervalTreeCollisionManager::update(CollisionObject* updated_obj) { AABB old_aabb; const AABB& new_aabb = updated_obj->getAABB(); for(int i = 0; i < 3; ++i) { std::map::const_iterator it = obj_interval_maps[i].find(updated_obj); interval_trees[i]->deleteNode(it->second); old_aabb.min_[i] = it->second->low; old_aabb.max_[i] = it->second->high; it->second->low = new_aabb.min_[i]; it->second->high = new_aabb.max_[i]; interval_trees[i]->insert(it->second); } EndPoint dummy; std::vector::iterator it; for(int i = 0; i < 3; ++i) { dummy.value = old_aabb.min_[i]; it = std::lower_bound(endpoints[i].begin(), endpoints[i].end(), dummy); for(; it != endpoints[i].end(); ++it) { if(it->obj == updated_obj && it->minmax == 0) { it->value = new_aabb.min_[i]; break; } } dummy.value = old_aabb.max_[i]; it = std::lower_bound(endpoints[i].begin(), endpoints[i].end(), dummy); for(; it != endpoints[i].end(); ++it) { if(it->obj == updated_obj && it->minmax == 0) { it->value = new_aabb.max_[i]; break; } } std::sort(endpoints[i].begin(), endpoints[i].end()); } } void IntervalTreeCollisionManager::update(const std::vector& updated_objs) { for(size_t i = 0; i < updated_objs.size(); ++i) update(updated_objs[i]); } void IntervalTreeCollisionManager::clear() { endpoints[0].clear(); endpoints[1].clear(); endpoints[2].clear(); delete interval_trees[0]; interval_trees[0] = NULL; delete interval_trees[1]; interval_trees[1] = NULL; delete interval_trees[2]; interval_trees[2] = NULL; for(int i = 0; i < 3; ++i) { for(std::map::const_iterator it = obj_interval_maps[i].begin(), end = obj_interval_maps[i].end(); it != end; ++it) { delete it->second; } } for(int i = 0; i < 3; ++i) obj_interval_maps[i].clear(); setup_ = false; } void IntervalTreeCollisionManager::getObjects(std::vector& objs) const { objs.resize(endpoints[0].size() / 2); unsigned int j = 0; for(unsigned int i = 0, size = endpoints[0].size(); i < size; ++i) { if(endpoints[0][i].minmax == 0) { objs[j] = endpoints[0][i].obj; j++; } } } void IntervalTreeCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const { if(size() == 0) return; collide_(obj, cdata, callback); } bool IntervalTreeCollisionManager::collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const { static const unsigned int CUTOFF = 100; std::deque results0, results1, results2; results0 = interval_trees[0]->query(obj->getAABB().min_[0], obj->getAABB().max_[0]); if(results0.size() > CUTOFF) { results1 = interval_trees[1]->query(obj->getAABB().min_[1], obj->getAABB().max_[1]); if(results1.size() > CUTOFF) { results2 = interval_trees[2]->query(obj->getAABB().min_[2], obj->getAABB().max_[2]); if(results2.size() > CUTOFF) { int d1 = results0.size(); int d2 = results1.size(); int d3 = results2.size(); if(d1 >= d2 && d1 >= d3) return checkColl(results0.begin(), results0.end(), obj, cdata, callback); else if(d2 >= d1 && d2 >= d3) return checkColl(results1.begin(), results1.end(), obj, cdata, callback); else return checkColl(results2.begin(), results2.end(), obj, cdata, callback); } else return checkColl(results2.begin(), results2.end(), obj, cdata, callback); } else return checkColl(results1.begin(), results1.end(), obj, cdata, callback); } else return checkColl(results0.begin(), results0.end(), obj, cdata, callback); } void IntervalTreeCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const { if(size() == 0) return; FCL_REAL min_dist = std::numeric_limits::max(); distance_(obj, cdata, callback, min_dist); } bool IntervalTreeCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const { static const unsigned int CUTOFF = 100; Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5; AABB aabb = obj->getAABB(); if(min_dist < std::numeric_limits::max()) { Vec3f min_dist_delta(min_dist, min_dist, min_dist); aabb.expand(min_dist_delta); } int status = 1; FCL_REAL old_min_distance; while(1) { bool dist_res = false; old_min_distance = min_dist; std::deque results0, results1, results2; results0 = interval_trees[0]->query(aabb.min_[0], aabb.max_[0]); if(results0.size() > CUTOFF) { results1 = interval_trees[1]->query(aabb.min_[1], aabb.max_[1]); if(results1.size() > CUTOFF) { results2 = interval_trees[2]->query(aabb.min_[2], aabb.max_[2]); if(results2.size() > CUTOFF) { int d1 = results0.size(); int d2 = results1.size(); int d3 = results2.size(); if(d1 >= d2 && d1 >= d3) dist_res = checkDist(results0.begin(), results0.end(), obj, cdata, callback, min_dist); else if(d2 >= d1 && d2 >= d3) dist_res = checkDist(results1.begin(), results1.end(), obj, cdata, callback, min_dist); else dist_res = checkDist(results2.begin(), results2.end(), obj, cdata, callback, min_dist); } else dist_res = checkDist(results2.begin(), results2.end(), obj, cdata, callback, min_dist); } else dist_res = checkDist(results1.begin(), results1.end(), obj, cdata, callback, min_dist); } else dist_res = checkDist(results0.begin(), results0.end(), obj, cdata, callback, min_dist); if(dist_res) return true; results0.clear(); results1.clear(); results2.clear(); if(status == 1) { if(old_min_distance < std::numeric_limits::max()) break; else { if(min_dist < old_min_distance) { Vec3f min_dist_delta(min_dist, min_dist, min_dist); aabb = AABB(obj->getAABB(), min_dist_delta); status = 0; } else { if(aabb.equal(obj->getAABB())) aabb.expand(delta); else aabb.expand(obj->getAABB(), 2.0); } } } else if(status == 0) break; } return false; } void IntervalTreeCollisionManager::collide(void* cdata, CollisionCallBack callback) const { if(size() == 0) return; std::set active; std::set > overlap; unsigned int n = endpoints[0].size(); double diff_x = endpoints[0][0].value - endpoints[0][n-1].value; double diff_y = endpoints[1][0].value - endpoints[1][n-1].value; double diff_z = endpoints[2][0].value - endpoints[2][n-1].value; int axis = 0; if(diff_y > diff_x && diff_y > diff_z) axis = 1; else if(diff_z > diff_y && diff_z > diff_x) axis = 2; for(unsigned int i = 0; i < n; ++i) { const EndPoint& endpoint = endpoints[axis][i]; CollisionObject* index = endpoint.obj; if(endpoint.minmax == 0) { std::set::iterator iter = active.begin(); std::set::iterator end = active.end(); for(; iter != end; ++iter) { CollisionObject* active_index = *iter; const AABB& b0 = active_index->getAABB(); const AABB& b1 = index->getAABB(); int axis2 = (axis + 1) % 3; int axis3 = (axis + 2) % 3; if(b0.axisOverlap(b1, axis2) && b0.axisOverlap(b1, axis3)) { std::pair >::iterator, bool> insert_res; if(active_index < index) insert_res = overlap.insert(std::make_pair(active_index, index)); else insert_res = overlap.insert(std::make_pair(index, active_index)); if(insert_res.second) { if(callback(active_index, index, cdata)) return; } } } active.insert(index); } else active.erase(index); } } void IntervalTreeCollisionManager::distance(void* cdata, DistanceCallBack callback) const { if(size() == 0) return; enable_tested_set_ = true; tested_set.clear(); FCL_REAL min_dist = std::numeric_limits::max(); for(size_t i = 0; i < endpoints[0].size(); ++i) if(distance_(endpoints[0][i].obj, cdata, callback, min_dist)) break; enable_tested_set_ = false; tested_set.clear(); } void IntervalTreeCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const { IntervalTreeCollisionManager* other_manager = static_cast(other_manager_); if((size() == 0) || (other_manager->size() == 0)) return; if(this == other_manager) { collide(cdata, callback); return; } if(this->size() < other_manager->size()) { for(size_t i = 0, size = endpoints[0].size(); i < size; ++i) if(other_manager->collide_(endpoints[0][i].obj, cdata, callback)) return; } else { for(size_t i = 0, size = other_manager->endpoints[0].size(); i < size; ++i) if(collide_(other_manager->endpoints[0][i].obj, cdata, callback)) return; } } void IntervalTreeCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const { IntervalTreeCollisionManager* other_manager = static_cast(other_manager_); if((size() == 0) || (other_manager->size() == 0)) return; if(this == other_manager) { distance(cdata, callback); return; } FCL_REAL min_dist = std::numeric_limits::max(); if(this->size() < other_manager->size()) { for(size_t i = 0, size = endpoints[0].size(); i < size; ++i) if(other_manager->distance_(endpoints[0][i].obj, cdata, callback, min_dist)) return; } else { for(size_t i = 0, size = other_manager->endpoints[0].size(); i < size; ++i) if(distance_(other_manager->endpoints[0][i].obj, cdata, callback, min_dist)) return; } } bool IntervalTreeCollisionManager::empty() const { return endpoints[0].empty(); } bool IntervalTreeCollisionManager::checkColl(std::deque::const_iterator pos_start, std::deque::const_iterator pos_end, CollisionObject* obj, void* cdata, CollisionCallBack callback) const { while(pos_start < pos_end) { SAPInterval* ivl = static_cast(*pos_start); if(ivl->obj != obj) { if(ivl->obj->getAABB().overlap(obj->getAABB())) { if(callback(ivl->obj, obj, cdata)) return true; } } pos_start++; } return false; } bool IntervalTreeCollisionManager::checkDist(std::deque::const_iterator pos_start, std::deque::const_iterator pos_end, CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const { while(pos_start < pos_end) { SAPInterval* ivl = static_cast(*pos_start); if(ivl->obj != obj) { if(!enable_tested_set_) { if(ivl->obj->getAABB().distance(obj->getAABB()) < min_dist) { if(callback(ivl->obj, obj, cdata, min_dist)) return true; } } else { if(!inTestedSet(ivl->obj, obj)) { if(ivl->obj->getAABB().distance(obj->getAABB()) < min_dist) { if(callback(ivl->obj, obj, cdata, min_dist)) return true; } insertTestedSet(ivl->obj, obj); } } } pos_start++; } return false; } } fcl-0.5.0/src/broadphase/broadphase_spatialhash.cpp000066400000000000000000000034621274356571000223730ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/broadphase/broadphase_spatialhash.h" namespace fcl { } fcl-0.5.0/src/broadphase/hierarchy_tree.cpp000066400000000000000000000107361274356571000207010ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/broadphase/hierarchy_tree.h" namespace fcl { template<> size_t select(const NodeBase& node, const NodeBase& node1, const NodeBase& node2) { const AABB& bv = node.bv; const AABB& bv1 = node1.bv; const AABB& bv2 = node2.bv; Vec3f v = bv.min_ + bv.max_; Vec3f v1 = v - (bv1.min_ + bv1.max_); Vec3f v2 = v - (bv2.min_ + bv2.max_); FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); return (d1 < d2) ? 0 : 1; } template<> size_t select(const AABB& query, const NodeBase& node1, const NodeBase& node2) { const AABB& bv = query; const AABB& bv1 = node1.bv; const AABB& bv2 = node2.bv; Vec3f v = bv.min_ + bv.max_; Vec3f v1 = v - (bv1.min_ + bv1.max_); Vec3f v2 = v - (bv2.min_ + bv2.max_); FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); return (d1 < d2) ? 0 : 1; } template<> bool HierarchyTree::update(NodeBase* leaf, const AABB& bv_, const Vec3f& vel, FCL_REAL margin) { AABB bv(bv_); if(leaf->bv.contain(bv)) return false; Vec3f marginv(margin); bv.min_ -= marginv; bv.max_ += marginv; if(vel[0] > 0) bv.max_[0] += vel[0]; else bv.min_[0] += vel[0]; if(vel[1] > 0) bv.max_[1] += vel[1]; else bv.min_[1] += vel[1]; if(vel[2] > 0) bv.max_[2] += vel[2]; else bv.max_[2] += vel[2]; update(leaf, bv); return true; } template<> bool HierarchyTree::update(NodeBase* leaf, const AABB& bv_, const Vec3f& vel) { AABB bv(bv_); if(leaf->bv.contain(bv)) return false; if(vel[0] > 0) bv.max_[0] += vel[0]; else bv.min_[0] += vel[0]; if(vel[1] > 0) bv.max_[1] += vel[1]; else bv.min_[1] += vel[1]; if(vel[2] > 0) bv.max_[2] += vel[2]; else bv.max_[2] += vel[2]; update(leaf, bv); return true; } namespace implementation_array { template<> size_t select(size_t query, size_t node1, size_t node2, NodeBase* nodes) { const AABB& bv = nodes[query].bv; const AABB& bv1 = nodes[node1].bv; const AABB& bv2 = nodes[node2].bv; Vec3f v = bv.min_ + bv.max_; Vec3f v1 = v - (bv1.min_ + bv1.max_); Vec3f v2 = v - (bv2.min_ + bv2.max_); FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); return (d1 < d2) ? 0 : 1; } template<> size_t select(const AABB& query, size_t node1, size_t node2, NodeBase* nodes) { const AABB& bv = query; const AABB& bv1 = nodes[node1].bv; const AABB& bv2 = nodes[node2].bv; Vec3f v = bv.min_ + bv.max_; Vec3f v1 = v - (bv1.min_ + bv1.max_); Vec3f v2 = v - (bv2.min_ + bv2.max_); FCL_REAL d1 = fabs(v1[0]) + fabs(v1[1]) + fabs(v1[2]); FCL_REAL d2 = fabs(v2[0]) + fabs(v2[1]) + fabs(v2[2]); return (d1 < d2) ? 0 : 1; } } } fcl-0.5.0/src/broadphase/interval_tree.cpp000066400000000000000000000316401274356571000205440ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/broadphase/interval_tree.h" #include #include #include namespace fcl { IntervalTreeNode::IntervalTreeNode(){} IntervalTreeNode::IntervalTreeNode(SimpleInterval* new_interval) : stored_interval (new_interval), key(new_interval->low), high(new_interval->high), max_high(high) {} IntervalTreeNode::~IntervalTreeNode() {} /// @brief Class describes the information needed when we take the /// right branch in searching for intervals but possibly come back /// and check the left branch as well. struct it_recursion_node { public: IntervalTreeNode* start_node; unsigned int parent_index; bool try_right_branch; }; IntervalTree::IntervalTree() { nil = new IntervalTreeNode; nil->left = nil->right = nil->parent = nil; nil->red = false; nil->key = nil->high = nil->max_high = -std::numeric_limits::max(); nil->stored_interval = NULL; root = new IntervalTreeNode; root->parent = root->left = root->right = nil; root->key = root->high = root->max_high = std::numeric_limits::max(); root->red = false; root->stored_interval = NULL; /// the following are used for the query function recursion_node_stack_size = 128; recursion_node_stack = (it_recursion_node*)malloc(recursion_node_stack_size*sizeof(it_recursion_node)); recursion_node_stack_top = 1; recursion_node_stack[0].start_node = NULL; } IntervalTree::~IntervalTree() { IntervalTreeNode* x = root->left; std::deque nodes_to_free; if(x != nil) { if(x->left != nil) { nodes_to_free.push_back(x->left); } if(x->right != nil) { nodes_to_free.push_back(x->right); } delete x; while( nodes_to_free.size() > 0) { x = nodes_to_free.back(); nodes_to_free.pop_back(); if(x->left != nil) { nodes_to_free.push_back(x->left); } if(x->right != nil) { nodes_to_free.push_back(x->right); } delete x; } } delete nil; delete root; free(recursion_node_stack); } void IntervalTree::leftRotate(IntervalTreeNode* x) { IntervalTreeNode* y; y = x->right; x->right = y->left; if(y->left != nil) y->left->parent = x; y->parent = x->parent; if(x == x->parent->left) x->parent->left = y; else x->parent->right = y; y->left = x; x->parent = y; x->max_high = std::max(x->left->max_high, std::max(x->right->max_high, x->high)); y->max_high = std::max(x->max_high, std::max(y->right->max_high, y->high)); } void IntervalTree::rightRotate(IntervalTreeNode* y) { IntervalTreeNode* x; x = y->left; y->left = x->right; if(nil != x->right) x->right->parent = y; x->parent = y->parent; if(y == y->parent->left) y->parent->left = x; else y->parent->right = x; x->right = y; y->parent = x; y->max_high = std::max(y->left->max_high, std::max(y->right->max_high, y->high)); x->max_high = std::max(x->left->max_high, std::max(y->max_high, x->high)); } /// @brief Inserts z into the tree as if it were a regular binary tree void IntervalTree::recursiveInsert(IntervalTreeNode* z) { IntervalTreeNode* x; IntervalTreeNode* y; z->left = z->right = nil; y = root; x = root->left; while(x != nil) { y = x; if(x->key > z->key) x = x->left; else x = x->right; } z->parent = y; if((y == root) || (y->key > z->key)) y->left = z; else y->right = z; } void IntervalTree::fixupMaxHigh(IntervalTreeNode* x) { while(x != root) { x->max_high = std::max(x->high, std::max(x->left->max_high, x->right->max_high)); x = x->parent; } } IntervalTreeNode* IntervalTree::insert(SimpleInterval* new_interval) { IntervalTreeNode* y; IntervalTreeNode* x; IntervalTreeNode* new_node; x = new IntervalTreeNode(new_interval); recursiveInsert(x); fixupMaxHigh(x->parent); new_node = x; x->red = true; while(x->parent->red) { /// use sentinel instead of checking for root if(x->parent == x->parent->parent->left) { y = x->parent->parent->right; if(y->red) { x->parent->red = true; y->red = true; x->parent->parent->red = true; x = x->parent->parent; } else { if(x == x->parent->right) { x = x->parent; leftRotate(x); } x->parent->red = false; x->parent->parent->red = true; rightRotate(x->parent->parent); } } else { y = x->parent->parent->left; if(y->red) { x->parent->red = false; y->red = false; x->parent->parent->red = true; x = x->parent->parent; } else { if(x == x->parent->left) { x = x->parent; rightRotate(x); } x->parent->red = false; x->parent->parent->red = true; leftRotate(x->parent->parent); } } } root->left->red = false; return new_node; } IntervalTreeNode* IntervalTree::getSuccessor(IntervalTreeNode* x) const { IntervalTreeNode* y; if(nil != (y = x->right)) { while(y->left != nil) y = y->left; return y; } else { y = x->parent; while(x == y->right) { x = y; y = y->parent; } if(y == root) return nil; return y; } } IntervalTreeNode* IntervalTree::getPredecessor(IntervalTreeNode* x) const { IntervalTreeNode* y; if(nil != (y = x->left)) { while(y->right != nil) y = y->right; return y; } else { y = x->parent; while(x == y->left) { if(y == root) return nil; x = y; y = y->parent; } return y; } } void IntervalTreeNode::print(IntervalTreeNode* nil, IntervalTreeNode* root) const { stored_interval->print(); std::cout << ", k = " << key << ", h = " << high << ", mH = " << max_high; std::cout << " l->key = "; if(left == nil) std::cout << "NULL"; else std::cout << left->key; std::cout << " r->key = "; if(right == nil) std::cout << "NULL"; else std::cout << right->key; std::cout << " p->key = "; if(parent == root) std::cout << "NULL"; else std::cout << parent->key; std::cout << " red = " << (int)red << std::endl; } void IntervalTree::recursivePrint(IntervalTreeNode* x) const { if(x != nil) { recursivePrint(x->left); x->print(nil,root); recursivePrint(x->right); } } void IntervalTree::print() const { recursivePrint(root->left); } void IntervalTree::deleteFixup(IntervalTreeNode* x) { IntervalTreeNode* w; IntervalTreeNode* root_left_node = root->left; while((!x->red) && (root_left_node != x)) { if(x == x->parent->left) { w = x->parent->right; if(w->red) { w->red = false; x->parent->red = true; leftRotate(x->parent); w = x->parent->right; } if((!w->right->red) && (!w->left->red)) { w->red = true; x = x->parent; } else { if(!w->right->red) { w->left->red = false; w->red = true; rightRotate(w); w = x->parent->right; } w->red = x->parent->red; x->parent->red = false; w->right->red = false; leftRotate(x->parent); x = root_left_node; } } else { w = x->parent->left; if(w->red) { w->red = false; x->parent->red = true; rightRotate(x->parent); w = x->parent->left; } if((!w->right->red) && (!w->left->red)) { w->red = true; x = x->parent; } else { if(!w->left->red) { w->right->red = false; w->red = true; leftRotate(w); w = x->parent->left; } w->red = x->parent->red; x->parent->red = false; w->left->red = false; rightRotate(x->parent); x = root_left_node; } } } x->red = false; } void IntervalTree::deleteNode(SimpleInterval* ivl) { IntervalTreeNode* node = recursiveSearch(root, ivl); if(node) deleteNode(node); } IntervalTreeNode* IntervalTree::recursiveSearch(IntervalTreeNode* node, SimpleInterval* ivl) const { if(node != nil) { if(node->stored_interval == ivl) return node; IntervalTreeNode* left = recursiveSearch(node->left, ivl); if(left != nil) return left; IntervalTreeNode* right = recursiveSearch(node->right, ivl); if(right != nil) return right; } return nil; } SimpleInterval* IntervalTree::deleteNode(IntervalTreeNode* z) { IntervalTreeNode* y; IntervalTreeNode* x; SimpleInterval* node_to_delete = z->stored_interval; y= ((z->left == nil) || (z->right == nil)) ? z : getSuccessor(z); x= (y->left == nil) ? y->right : y->left; if(root == (x->parent = y->parent)) { root->left = x; } else { if(y == y->parent->left) { y->parent->left = x; } else { y->parent->right = x; } } /// @brief y should not be nil in this case /// y is the node to splice out and x is its child if(y != z) { y->max_high = -std::numeric_limits::max(); y->left = z->left; y->right = z->right; y->parent = z->parent; z->left->parent = z->right->parent = y; if(z == z->parent->left) z->parent->left = y; else z->parent->right = y; fixupMaxHigh(x->parent); if(!(y->red)) { y->red = z->red; deleteFixup(x); } else y->red = z->red; delete z; } else { fixupMaxHigh(x->parent); if(!(y->red)) deleteFixup(x); delete y; } return node_to_delete; } /// @brief returns 1 if the intervals overlap, and 0 otherwise bool overlap(double a1, double a2, double b1, double b2) { if(a1 <= b1) { return (b1 <= a2); } else { return (a1 <= b2); } } std::deque IntervalTree::query(double low, double high) { std::deque result_stack; IntervalTreeNode* x = root->left; bool run = (x != nil); current_parent = 0; while(run) { if(overlap(low,high,x->key,x->high)) { result_stack.push_back(x->stored_interval); recursion_node_stack[current_parent].try_right_branch = true; } if(x->left->max_high >= low) { if(recursion_node_stack_top == recursion_node_stack_size) { recursion_node_stack_size *= 2; recursion_node_stack = (it_recursion_node *)realloc(recursion_node_stack, recursion_node_stack_size * sizeof(it_recursion_node)); if(recursion_node_stack == NULL) exit(1); } recursion_node_stack[recursion_node_stack_top].start_node = x; recursion_node_stack[recursion_node_stack_top].try_right_branch = false; recursion_node_stack[recursion_node_stack_top].parent_index = current_parent; current_parent = recursion_node_stack_top++; x = x->left; } else x = x->right; run = (x != nil); while((!run) && (recursion_node_stack_top > 1)) { if(recursion_node_stack[--recursion_node_stack_top].try_right_branch) { x=recursion_node_stack[recursion_node_stack_top].start_node->right; current_parent=recursion_node_stack[recursion_node_stack_top].parent_index; recursion_node_stack[current_parent].try_right_branch = true; run = (x != nil); } } } return result_stack; } } fcl-0.5.0/src/ccd/000077500000000000000000000000001274356571000136125ustar00rootroot00000000000000fcl-0.5.0/src/ccd/conservative_advancement.cpp000066400000000000000000001333321274356571000214000ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/ccd/conservative_advancement.h" #include "fcl/ccd/motion.h" #include "fcl/collision_node.h" #include "fcl/traversal/traversal_node_bvhs.h" #include "fcl/traversal/traversal_node_setup.h" #include "fcl/traversal/traversal_recurse.h" #include "fcl/collision.h" namespace fcl { template bool conservativeAdvancement(const BVHModel& o1, const MotionBase* motion1, const BVHModel& o2, const MotionBase* motion2, const CollisionRequest& request, CollisionResult& result, FCL_REAL& toc) { Transform3f tf1, tf2; motion1->getCurrentTransform(tf1); motion2->getCurrentTransform(tf2); // whether the first start configuration is in collision if(collide(&o1, tf1, &o2, tf2, request, result)) { toc = 0; return true; } BVHModel* o1_tmp = new BVHModel(o1); BVHModel* o2_tmp = new BVHModel(o2); MeshConservativeAdvancementTraversalNode node; node.motion1 = motion1; node.motion2 = motion2; do { // repeatedly update mesh to global coordinate, so time consuming initialize(node, *o1_tmp, tf1, *o2_tmp, tf2); node.delta_t = 1; node.min_distance = std::numeric_limits::max(); distanceRecurse(&node, 0, 0, NULL); if(node.delta_t <= node.t_err) { // std::cout << node.delta_t << " " << node.t_err << std::endl; break; } node.toc += node.delta_t; if(node.toc > 1) { node.toc = 1; break; } node.motion1->integrate(node.toc); node.motion2->integrate(node.toc); motion1->getCurrentTransform(tf1); motion2->getCurrentTransform(tf2); } while(1); delete o1_tmp; delete o2_tmp; toc = node.toc; if(node.toc < 1) return true; return false; } namespace details { template bool conservativeAdvancementMeshOriented(const BVHModel& o1, const MotionBase* motion1, const BVHModel& o2, const MotionBase* motion2, const CollisionRequest& request, CollisionResult& result, FCL_REAL& toc) { Transform3f tf1, tf2; motion1->getCurrentTransform(tf1); motion2->getCurrentTransform(tf2); // whether the first start configuration is in collision if(collide(&o1, tf1, &o2, tf2, request, result)) { toc = 0; return true; } ConservativeAdvancementOrientedNode node; initialize(node, o1, tf1, o2, tf2); node.motion1 = motion1; node.motion2 = motion2; do { node.motion1->getCurrentTransform(tf1); node.motion2->getCurrentTransform(tf2); // compute the transformation from 1 to 2 Transform3f tf; relativeTransform(tf1, tf2, tf); node.R = tf.getRotation(); node.T = tf.getTranslation(); node.delta_t = 1; node.min_distance = std::numeric_limits::max(); distanceRecurse(&node, 0, 0, NULL); if(node.delta_t <= node.t_err) { // std::cout << node.delta_t << " " << node.t_err << std::endl; break; } node.toc += node.delta_t; if(node.toc > 1) { node.toc = 1; break; } node.motion1->integrate(node.toc); node.motion2->integrate(node.toc); } while(1); toc = node.toc; if(node.toc < 1) return true; return false; } } template<> bool conservativeAdvancement(const BVHModel& o1, const MotionBase* motion1, const BVHModel& o2, const MotionBase* motion2, const CollisionRequest& request, CollisionResult& result, FCL_REAL& toc); template<> bool conservativeAdvancement(const BVHModel& o1, const MotionBase* motion1, const BVHModel& o2, const MotionBase* motion2, const CollisionRequest& request, CollisionResult& result, FCL_REAL& toc); template bool conservativeAdvancement(const S1& o1, const MotionBase* motion1, const S2& o2, const MotionBase* motion2, const NarrowPhaseSolver* solver, const CollisionRequest& request, CollisionResult& result, FCL_REAL& toc) { Transform3f tf1, tf2; motion1->getCurrentTransform(tf1); motion2->getCurrentTransform(tf2); // whether the first start configuration is in collision if(collide(&o1, tf1, &o2, tf2, request, result)) { toc = 0; return true; } ShapeConservativeAdvancementTraversalNode node; initialize(node, o1, tf1, o2, tf2, solver); node.motion1 = motion1; node.motion2 = motion2; do { motion1->getCurrentTransform(tf1); motion2->getCurrentTransform(tf2); node.tf1 = tf1; node.tf2 = tf2; node.delta_t = 1; node.min_distance = std::numeric_limits::max(); distanceRecurse(&node, 0, 0, NULL); if(node.delta_t <= node.t_err) { // std::cout << node.delta_t << " " << node.t_err << std::endl; break; } node.toc += node.delta_t; if(node.toc > 1) { node.toc = 1; break; } node.motion1->integrate(node.toc); node.motion2->integrate(node.toc); } while(1); toc = node.toc; if(node.toc < 1) return true; return false; } template bool conservativeAdvancement(const BVHModel& o1, const MotionBase* motion1, const S& o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result, FCL_REAL& toc) { Transform3f tf1, tf2; motion1->getCurrentTransform(tf1); motion2->getCurrentTransform(tf2); if(collide(&o1, tf1, &o2, tf2, request, result)) { toc = 0; return true; } BVHModel* o1_tmp = new BVHModel(o1); MeshShapeConservativeAdvancementTraversalNode node; node.motion1 = motion1; node.motion2 = motion2; do { // initialize update mesh to global coordinate, so time consuming initialize(node, *o1_tmp, tf1, o2, tf2, nsolver); node.delta_t = 1; node.min_distance = std::numeric_limits::max(); distanceRecurse(&node, 0, 0, NULL); if(node.delta_t <= node.t_err) { break; } node.toc += node.delta_t; if(node.toc > 1) { node.toc = 1; break; } node.motion1->integrate(node.toc); node.motion2->integrate(node.toc); motion1->getCurrentTransform(tf1); motion2->getCurrentTransform(tf2); } while(1); delete o1_tmp; toc = node.toc; if(node.toc < 1) return true; return false; } namespace details { template bool conservativeAdvancementMeshShapeOriented(const BVHModel& o1, const MotionBase* motion1, const S& o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result, FCL_REAL& toc) { Transform3f tf1, tf2; motion1->getCurrentTransform(tf1); motion2->getCurrentTransform(tf2); if(collide(&o1, tf1, &o2, tf2, request, result)) { toc = 0; return true; } ConservativeAdvancementOrientedNode node; initialize(node, o1, tf1, o2, tf2, nsolver); node.motion1 = motion1; node.motion2 = motion2; do { node.motion1->getCurrentTransform(tf1); node.motion2->getCurrentTransform(tf2); node.tf1 = tf1; node.tf2 = tf2; node.delta_t = 1; node.min_distance = std::numeric_limits::max(); distanceRecurse(&node, 0, 0, NULL); if(node.delta_t <= node.t_err) { break; } node.toc += node.delta_t; if(node.toc > 1) { node.toc = 1; break; } node.motion1->integrate(node.toc); node.motion2->integrate(node.toc); } while(1); toc = node.toc; if(node.toc < 1) return true; return false; } } template bool conservativeAdvancement(const BVHModel& o1, const MotionBase* motion1, const S& o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result, FCL_REAL& toc) { return details::conservativeAdvancementMeshShapeOriented >(o1, motion1, o2, motion2, nsolver, request, result, toc); } template bool conservativeAdvancement(const BVHModel& o1, const MotionBase* motion1, const S& o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result, FCL_REAL& toc) { return details::conservativeAdvancementMeshShapeOriented >(o1, motion1, o2, motion2, nsolver, request, result, toc); } template bool conservativeAdvancement(const S& o1, const MotionBase* motion1, const BVHModel& o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result, FCL_REAL& toc) { Transform3f tf1, tf2; motion1->getCurrentTransform(tf1); motion2->getCurrentTransform(tf2); if(collide(&o1, tf1, &o2, tf2, request, result)) { toc = 0; return true; } BVHModel* o2_tmp = new BVHModel(o2); ShapeMeshConservativeAdvancementTraversalNode node; node.motion1 = motion1; node.motion2 = motion2; do { // initialize update mesh to global coordinate, so time consuming initialize(node, o1, tf1, *o2_tmp, tf2, nsolver); node.delta_t = 1; node.min_distance = std::numeric_limits::max(); distanceRecurse(&node, 0, 0, NULL); if(node.delta_t <= node.t_err) { break; } node.toc += node.delta_t; if(node.toc > 1) { node.toc = 1; break; } node.motion1->integrate(node.toc); node.motion2->integrate(node.toc); motion1->getCurrentTransform(tf1); motion2->getCurrentTransform(tf2); } while(1); delete o2_tmp; toc = node.toc; if(node.toc < 1) return true; return false; } namespace details { template bool conservativeAdvancementShapeMeshOriented(const S& o1, const MotionBase* motion1, const BVHModel& o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result, FCL_REAL& toc) { Transform3f tf1, tf2; motion1->getCurrentTransform(tf1); motion2->getCurrentTransform(tf2); if(collide(&o1, tf1, &o2, tf2, request, result)) { toc = 0; return true; } ConservativeAdvancementOrientedNode node; initialize(node, o1, tf1, o2, tf2, nsolver); node.motion1 = motion1; node.motion2 = motion2; do { node.motion1->getCurrentTransform(tf1); node.motion2->getCurrentTransform(tf2); node.tf1 = tf1; node.tf2 = tf2; node.delta_t = 1; node.min_distance = std::numeric_limits::max(); distanceRecurse(&node, 0, 0, NULL); if(node.delta_t <= node.t_err) { break; } node.toc += node.delta_t; if(node.toc > 1) { node.toc = 1; break; } node.motion1->integrate(node.toc); node.motion2->integrate(node.toc); } while(1); toc = node.toc; if(node.toc < 1) return true; return false; } } template bool conservativeAdvancement(const S& o1, const MotionBase* motion1, const BVHModel& o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result, FCL_REAL& toc) { return details::conservativeAdvancementShapeMeshOriented >(o1, motion1, o2, motion2, nsolver, request, result, toc); } template bool conservativeAdvancement(const S& o1, const MotionBase* motion1, const BVHModel& o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result, FCL_REAL& toc) { return details::conservativeAdvancementShapeMeshOriented >(o1, motion1, o2, motion2, nsolver, request, result, toc); } template<> bool conservativeAdvancement(const BVHModel& o1, const MotionBase* motion1, const BVHModel& o2, const MotionBase* motion2, const CollisionRequest& request, CollisionResult& result, FCL_REAL& toc) { return details::conservativeAdvancementMeshOriented(o1, motion1, o2, motion2, request, result, toc); } template<> bool conservativeAdvancement(const BVHModel& o1, const MotionBase* motion1, const BVHModel& o2, const MotionBase* motion2, const CollisionRequest& request, CollisionResult& result, FCL_REAL& toc) { return details::conservativeAdvancementMeshOriented(o1, motion1, o2, motion2, request, result, toc); } template FCL_REAL BVHConservativeAdvancement(const CollisionGeometry* o1, const MotionBase* motion1, const CollisionGeometry* o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result) { const BVHModel* obj1 = static_cast*>(o1); const BVHModel* obj2 = static_cast*>(o2); CollisionRequest c_request; CollisionResult c_result; FCL_REAL toc; bool is_collide = conservativeAdvancement(*obj1, motion1, *obj2, motion2, c_request, c_result, toc); result.is_collide = is_collide; result.time_of_contact = toc; return toc; } template FCL_REAL ShapeConservativeAdvancement(const CollisionGeometry* o1, const MotionBase* motion1, const CollisionGeometry* o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result) { const S1* obj1 = static_cast(o1); const S2* obj2 = static_cast(o2); CollisionRequest c_request; CollisionResult c_result; FCL_REAL toc; bool is_collide = conservativeAdvancement(*obj1, motion1, *obj2, motion2, nsolver, c_request, c_result, toc); result.is_collide = is_collide; result.time_of_contact = toc; return toc; } template FCL_REAL ShapeBVHConservativeAdvancement(const CollisionGeometry* o1, const MotionBase* motion1, const CollisionGeometry* o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result) { const S* obj1 = static_cast(o1); const BVHModel* obj2 = static_cast*>(o2); CollisionRequest c_request; CollisionResult c_result; FCL_REAL toc; bool is_collide = conservativeAdvancement(*obj1, motion1, *obj2, motion2, nsolver, c_request, c_result, toc); result.is_collide = is_collide; result.time_of_contact = toc; return toc; } template FCL_REAL BVHShapeConservativeAdvancement(const CollisionGeometry* o1, const MotionBase* motion1, const CollisionGeometry* o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result) { const BVHModel* obj1 = static_cast*>(o1); const S* obj2 = static_cast(o2); CollisionRequest c_request; CollisionResult c_result; FCL_REAL toc; bool is_collide = conservativeAdvancement(*obj1, motion1, *obj2, motion2, nsolver, c_request, c_result, toc); result.is_collide = is_collide; result.time_of_contact = toc; return toc; } template ConservativeAdvancementFunctionMatrix::ConservativeAdvancementFunctionMatrix() { for(int i = 0; i < NODE_COUNT; ++i) { for(int j = 0; j < NODE_COUNT; ++j) conservative_advancement_matrix[i][j] = NULL; } conservative_advancement_matrix[GEOM_BOX][GEOM_BOX] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_BOX][GEOM_CONE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_CONE][GEOM_BOX] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_CONE][GEOM_CONE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_CONVEX][GEOM_HALFSPACE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_CONE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_CONVEX] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_PLANE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeConservativeAdvancement; conservative_advancement_matrix[BV_AABB][GEOM_BOX] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_AABB][GEOM_CONE] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_OBB][GEOM_BOX] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_OBB][GEOM_CONE] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_OBBRSS][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_OBBRSS][GEOM_CONE] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_RSS][GEOM_BOX] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_RSS][GEOM_CONE] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeConservativeAdvancement, Box, NarrowPhaseSolver>; conservative_advancement_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeConservativeAdvancement, Sphere, NarrowPhaseSolver>; conservative_advancement_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement, Capsule, NarrowPhaseSolver>; conservative_advancement_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeConservativeAdvancement, Cone, NarrowPhaseSolver>; conservative_advancement_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement, Cylinder, NarrowPhaseSolver>; conservative_advancement_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeConservativeAdvancement, Convex, NarrowPhaseSolver>; conservative_advancement_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeConservativeAdvancement, Plane, NarrowPhaseSolver>; conservative_advancement_matrix[BV_KDOP16][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement, Halfspace, NarrowPhaseSolver>; conservative_advancement_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeConservativeAdvancement, Box, NarrowPhaseSolver>; conservative_advancement_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeConservativeAdvancement, Sphere, NarrowPhaseSolver>; conservative_advancement_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement, Capsule, NarrowPhaseSolver>; conservative_advancement_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeConservativeAdvancement, Cone, NarrowPhaseSolver>; conservative_advancement_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement, Cylinder, NarrowPhaseSolver>; conservative_advancement_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeConservativeAdvancement, Convex, NarrowPhaseSolver>; conservative_advancement_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeConservativeAdvancement, Plane, NarrowPhaseSolver>; conservative_advancement_matrix[BV_KDOP18][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement, Halfspace, NarrowPhaseSolver>; conservative_advancement_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeConservativeAdvancement, Box, NarrowPhaseSolver>; conservative_advancement_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeConservativeAdvancement, Sphere, NarrowPhaseSolver>; conservative_advancement_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement, Capsule, NarrowPhaseSolver>; conservative_advancement_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeConservativeAdvancement, Cone, NarrowPhaseSolver>; conservative_advancement_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement, Cylinder, NarrowPhaseSolver>; conservative_advancement_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeConservativeAdvancement, Convex, NarrowPhaseSolver>; conservative_advancement_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeConservativeAdvancement, Plane, NarrowPhaseSolver>; conservative_advancement_matrix[BV_KDOP24][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement, Halfspace, NarrowPhaseSolver>; conservative_advancement_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeConservativeAdvancement; conservative_advancement_matrix[GEOM_BOX][BV_AABB] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_SPHERE][BV_AABB] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_CAPSULE][BV_AABB] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_CONE][BV_AABB] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_CYLINDER][BV_AABB] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_CONVEX][BV_AABB] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_PLANE][BV_AABB] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_HALFSPACE][BV_AABB] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_BOX][BV_OBB] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_SPHERE][BV_OBB] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_CAPSULE][BV_OBB] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_CONE][BV_OBB] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_CYLINDER][BV_OBB] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_CONVEX][BV_OBB] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_PLANE][BV_OBB] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_HALFSPACE][BV_OBB] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_BOX][BV_RSS] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_SPHERE][BV_RSS] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_CAPSULE][BV_RSS] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_CONE][BV_RSS] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_CYLINDER][BV_RSS] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_CONVEX][BV_RSS] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_PLANE][BV_RSS] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_HALFSPACE][BV_RSS] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_BOX][BV_OBBRSS] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_SPHERE][BV_OBBRSS] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_CAPSULE][BV_OBBRSS] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_CONE][BV_OBBRSS] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_CYLINDER][BV_OBBRSS] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_CONVEX][BV_OBBRSS] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_PLANE][BV_OBBRSS] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_HALFSPACE][BV_OBBRSS] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_BOX][BV_KDOP16] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; conservative_advancement_matrix[GEOM_SPHERE][BV_KDOP16] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; conservative_advancement_matrix[GEOM_CAPSULE][BV_KDOP16] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; conservative_advancement_matrix[GEOM_CONE][BV_KDOP16] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; conservative_advancement_matrix[GEOM_CYLINDER][BV_KDOP16] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; conservative_advancement_matrix[GEOM_CONVEX][BV_KDOP16] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; conservative_advancement_matrix[GEOM_PLANE][BV_KDOP16] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; conservative_advancement_matrix[GEOM_HALFSPACE][BV_KDOP16] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; conservative_advancement_matrix[GEOM_BOX][BV_KDOP18] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; conservative_advancement_matrix[GEOM_SPHERE][BV_KDOP18] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; conservative_advancement_matrix[GEOM_CAPSULE][BV_KDOP18] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; conservative_advancement_matrix[GEOM_CONE][BV_KDOP18] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; conservative_advancement_matrix[GEOM_CYLINDER][BV_KDOP18] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; conservative_advancement_matrix[GEOM_CONVEX][BV_KDOP18] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; conservative_advancement_matrix[GEOM_PLANE][BV_KDOP18] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; conservative_advancement_matrix[GEOM_HALFSPACE][BV_KDOP18] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; conservative_advancement_matrix[GEOM_BOX][BV_KDOP24] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; conservative_advancement_matrix[GEOM_SPHERE][BV_KDOP24] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; conservative_advancement_matrix[GEOM_CAPSULE][BV_KDOP24] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; conservative_advancement_matrix[GEOM_CONE][BV_KDOP24] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; conservative_advancement_matrix[GEOM_CYLINDER][BV_KDOP24] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; conservative_advancement_matrix[GEOM_CONVEX][BV_KDOP24] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; conservative_advancement_matrix[GEOM_PLANE][BV_KDOP24] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; conservative_advancement_matrix[GEOM_HALFSPACE][BV_KDOP24] = &ShapeBVHConservativeAdvancement, NarrowPhaseSolver>; conservative_advancement_matrix[GEOM_BOX][BV_kIOS] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_SPHERE][BV_kIOS] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_CAPSULE][BV_kIOS] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_CONE][BV_kIOS] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_CYLINDER][BV_kIOS] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_CONVEX][BV_kIOS] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_PLANE][BV_kIOS] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[GEOM_HALFSPACE][BV_kIOS] = &ShapeBVHConservativeAdvancement; conservative_advancement_matrix[BV_AABB][BV_AABB] = &BVHConservativeAdvancement; conservative_advancement_matrix[BV_OBB][BV_OBB] = &BVHConservativeAdvancement; conservative_advancement_matrix[BV_RSS][BV_RSS] = &BVHConservativeAdvancement; conservative_advancement_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHConservativeAdvancement; conservative_advancement_matrix[BV_KDOP16][BV_KDOP16] = &BVHConservativeAdvancement, NarrowPhaseSolver>; conservative_advancement_matrix[BV_KDOP18][BV_KDOP18] = &BVHConservativeAdvancement, NarrowPhaseSolver>; conservative_advancement_matrix[BV_KDOP24][BV_KDOP24] = &BVHConservativeAdvancement, NarrowPhaseSolver>; conservative_advancement_matrix[BV_kIOS][BV_kIOS] = &BVHConservativeAdvancement; } template struct ConservativeAdvancementFunctionMatrix; template struct ConservativeAdvancementFunctionMatrix; } fcl-0.5.0/src/ccd/interpolation/000077500000000000000000000000001274356571000165015ustar00rootroot00000000000000fcl-0.5.0/src/ccd/interpolation/interpolation.cpp000066400000000000000000000050201274356571000220710ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Dalibor Matura, Jia Pan */ #include "fcl/ccd/interpolation/interpolation.h" namespace fcl { Interpolation::Interpolation() : value_0_(0.0), value_1_(1.0) {} Interpolation::Interpolation(FCL_REAL start_value, FCL_REAL end_value) : value_0_(start_value), value_1_(end_value) {} void Interpolation::setStartValue(FCL_REAL start_value) { value_0_ = start_value; } void Interpolation::setEndValue(FCL_REAL end_value) { value_1_ = end_value; } bool Interpolation::operator == (const Interpolation& interpolation) const { return (this->getType() == interpolation.getType()) && (this->value_0_ == interpolation.value_0_) && (this->value_1_ == interpolation.value_1_); } bool Interpolation::operator != (const Interpolation& interpolation) const { return !(*this == interpolation); } } fcl-0.5.0/src/ccd/interpolation/interpolation_factory.cpp000066400000000000000000000051261274356571000236270ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Dalibor Matura, Jia Pan */ #include "fcl/ccd/interpolation/interpolation_factory.h" #include "fcl/ccd/interpolation/interpolation_linear.h" #include namespace fcl { InterpolationFactory::InterpolationFactory() { InterpolationLinear::registerToFactory(); } InterpolationFactory& InterpolationFactory::instance() { static InterpolationFactory instance; return instance; } void InterpolationFactory::registerClass(const InterpolationType type, const CreateFunction create_function) { this->creation_map_[type] = create_function; } std::shared_ptr InterpolationFactory::create(const InterpolationType type, const FCL_REAL start_value, const FCL_REAL end_value) { std::map::const_iterator it = creation_map_.find(type); assert(it != creation_map_.end()); return (it->second)(start_value, end_value); } } fcl-0.5.0/src/ccd/interpolation/interpolation_linear.cpp000066400000000000000000000060521274356571000234310ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Dalibor Matura, Jia Pan */ #include "fcl/ccd/interpolation/interpolation_linear.h" #include "fcl/ccd/interpolation/interpolation_factory.h" namespace fcl { InterpolationType interpolation_linear_type = LINEAR; InterpolationLinear::InterpolationLinear() : Interpolation(0.0, 1.0) {} InterpolationLinear::InterpolationLinear(FCL_REAL start_value, FCL_REAL end_value) : Interpolation(start_value, end_value) {} FCL_REAL InterpolationLinear::getValue(FCL_REAL time) const { return value_0_ + (value_1_ - value_0_) * time; } FCL_REAL InterpolationLinear::getValueLowerBound() const { return value_0_; } FCL_REAL InterpolationLinear::getValueUpperBound() const { return value_1_; } InterpolationType InterpolationLinear::getType() const { return interpolation_linear_type; } std::shared_ptr InterpolationLinear::create(FCL_REAL start_value, FCL_REAL end_value) { return std::shared_ptr(new InterpolationLinear(start_value, end_value) ); } void InterpolationLinear::registerToFactory() { InterpolationFactory::instance().registerClass(interpolation_linear_type, create); } FCL_REAL InterpolationLinear::getMovementLengthBound(FCL_REAL time) const { return getValueUpperBound() - getValue(time); } FCL_REAL InterpolationLinear::getVelocityBound(FCL_REAL time) const { return (value_1_ - value_0_); } } fcl-0.5.0/src/ccd/interval.cpp000066400000000000000000000121501274356571000161410ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/ccd/interval.h" #include namespace fcl { Interval bound(const Interval& i, FCL_REAL v) { Interval res = i; if(v < res.i_[0]) res.i_[0] = v; if(v > res.i_[1]) res.i_[1] = v; return res; } Interval bound(const Interval& i, const Interval& other) { Interval res = i; if(other.i_[0] < res.i_[0]) res.i_[0] = other.i_[0]; if(other.i_[1] > res.i_[1]) res.i_[1] = other.i_[1]; return res; } Interval Interval::operator * (const Interval& other) const { if(other.i_[0] >= 0) { if(i_[0] >= 0) return Interval(i_[0] * other.i_[0], i_[1] * other.i_[1]); if(i_[1] <= 0) return Interval(i_[0] * other.i_[1], i_[1] * other.i_[0]); return Interval(i_[0] * other.i_[1], i_[1] * other.i_[1]); } if(other.i_[1] <= 0) { if(i_[0] >= 0) return Interval(i_[1] * other.i_[0], i_[0] * other.i_[1]); if(i_[1] <= 0) return Interval(i_[1] * other.i_[1], i_[0] * other.i_[0]); return Interval(i_[1] * other.i_[0], i_[0] * other.i_[0]); } if(i_[0] >= 0) return Interval(i_[1] * other.i_[0], i_[1] * other.i_[1]); if(i_[1] <= 0) return Interval(i_[0] * other.i_[1], i_[0] * other.i_[0]); FCL_REAL v00 = i_[0] * other.i_[0]; FCL_REAL v11 = i_[1] * other.i_[1]; if(v00 <= v11) { FCL_REAL v01 = i_[0] * other.i_[1]; FCL_REAL v10 = i_[1] * other.i_[0]; if(v01 < v10) return Interval(v01, v11); return Interval(v10, v11); } FCL_REAL v01 = i_[0] * other.i_[1]; FCL_REAL v10 = i_[1] * other.i_[0]; if(v01 < v10) return Interval(v01, v00); return Interval(v10, v00); } Interval& Interval::operator *= (const Interval& other) { if(other.i_[0] >= 0) { if(i_[0] >= 0) { i_[0] *= other.i_[0]; i_[1] *= other.i_[1]; } else if(i_[1] <= 0) { i_[0] *= other.i_[1]; i_[1] *= other.i_[0]; } else { i_[0] *= other.i_[1]; i_[1] *= other.i_[1]; } return *this; } if(other.i_[1] <= 0) { if(i_[0] >= 0) { FCL_REAL tmp = i_[0]; i_[0] = i_[1] * other.i_[0]; i_[1] = tmp * other.i_[1]; } else if(i_[1] <= 0) { FCL_REAL tmp = i_[0]; i_[0] = i_[1] * other.i_[1]; i_[1] = tmp * other.i_[0]; } else { FCL_REAL tmp = i_[0]; i_[0] = i_[1] * other.i_[0]; i_[1] = tmp * other.i_[0]; } return *this; } if(i_[0] >= 0) { i_[0] = i_[1] * other.i_[0]; i_[1] *= other.i_[1]; return *this; } if(i_[1] <= 0) { i_[1] = i_[0] * other.i_[0]; i_[0] *= other.i_[1]; return *this; } FCL_REAL v00 = i_[0] * other.i_[0]; FCL_REAL v11 = i_[1] * other.i_[1]; if(v00 <= v11) { FCL_REAL v01 = i_[0] * other.i_[1]; FCL_REAL v10 = i_[1] * other.i_[0]; if(v01 < v10) { i_[0] = v01; i_[1] = v11; } else { i_[0] = v10; i_[1] = v11; } return *this; } FCL_REAL v01 = i_[0] * other.i_[1]; FCL_REAL v10 = i_[1] * other.i_[0]; if(v01 < v10) { i_[0] = v01; i_[1] = v00; } else { i_[0] = v10; i_[1] = v00; } return *this; } Interval Interval::operator / (const Interval& other) const { return *this * Interval(1.0 / other.i_[1], 1.0 / other.i_[0]); } Interval& Interval::operator /= (const Interval& other) { *this *= Interval(1.0 / other.i_[1], 1.0 / other.i_[0]); return *this; } void Interval::print() const { std::cout << "[" << i_[0] << ", " << i_[1] << "]" << std::endl; } } fcl-0.5.0/src/ccd/interval_matrix.cpp000066400000000000000000000162411274356571000175320ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/ccd/interval_matrix.h" #include namespace fcl { IMatrix3::IMatrix3() {} IMatrix3::IMatrix3(FCL_REAL v) { v_[0].setValue(v); v_[1].setValue(v); v_[2].setValue(v); } IMatrix3::IMatrix3(const Matrix3f& m) { v_[0] = m.getRow(0); v_[1] = m.getRow(1); v_[2] = m.getRow(2); } IMatrix3::IMatrix3(FCL_REAL m[3][3][2]) { v_[0].setValue(m[0]); v_[1].setValue(m[1]); v_[2].setValue(m[2]); } IMatrix3::IMatrix3(FCL_REAL m[3][3]) { v_[0].setValue(m[0]); v_[1].setValue(m[1]); v_[2].setValue(m[2]); } IMatrix3::IMatrix3(Interval m[3][3]) { v_[0].setValue(m[0]); v_[1].setValue(m[1]); v_[2].setValue(m[2]); } IMatrix3::IMatrix3(const IVector3& v1, const IVector3& v2, const IVector3& v3) { v_[0] = v1; v_[1] = v2; v_[2] = v3; } void IMatrix3::setIdentity() { v_[0].setValue(1, 0, 0); v_[1].setValue(0, 1, 0); v_[2].setValue(0, 0, 1); } IVector3 IMatrix3::getColumn(size_t i) const { return IVector3(v_[0][i], v_[1][i], v_[2][i]); } const IVector3& IMatrix3::getRow(size_t i) const { return v_[i]; } Vec3f IMatrix3::getColumnLow(size_t i) const { return Vec3f(v_[0][i][0], v_[1][i][0], v_[2][i][0]); } Vec3f IMatrix3::getRowLow(size_t i) const { return Vec3f(v_[i][0][0], v_[i][1][0], v_[i][2][0]); } Vec3f IMatrix3::getColumnHigh(size_t i) const { return Vec3f(v_[0][i][1], v_[1][i][1], v_[2][i][1]); } Vec3f IMatrix3::getRowHigh(size_t i) const { return Vec3f(v_[i][0][1], v_[i][1][1], v_[i][2][1]); } Matrix3f IMatrix3::getLow() const { return Matrix3f(v_[0][0][0], v_[0][1][0], v_[0][2][0], v_[1][0][0], v_[1][1][0], v_[1][2][0], v_[2][0][0], v_[2][1][0], v_[2][2][0]); } Matrix3f IMatrix3::getHigh() const { return Matrix3f(v_[0][0][1], v_[0][1][1], v_[0][2][1], v_[1][0][1], v_[1][1][1], v_[1][2][1], v_[2][0][1], v_[2][1][1], v_[2][2][1]); } IMatrix3 IMatrix3::operator * (const Matrix3f& m) const { const Vec3f& mc0 = m.getColumn(0); const Vec3f& mc1 = m.getColumn(1); const Vec3f& mc2 = m.getColumn(2); return IMatrix3(IVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)), IVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)), IVector3(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2))); } IVector3 IMatrix3::operator * (const Vec3f& v) const { return IVector3(v_[0].dot(v), v_[1].dot(v), v_[2].dot(v)); } IVector3 IMatrix3::operator * (const IVector3& v) const { return IVector3(v_[0].dot(v), v_[1].dot(v), v_[2].dot(v)); } IMatrix3 IMatrix3::operator * (const IMatrix3& m) const { const IVector3& mc0 = m.getColumn(0); const IVector3& mc1 = m.getColumn(1); const IVector3& mc2 = m.getColumn(2); return IMatrix3(IVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)), IVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)), IVector3(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2))); } IMatrix3& IMatrix3::operator *= (const Matrix3f& m) { const Vec3f& mc0 = m.getColumn(0); const Vec3f& mc1 = m.getColumn(1); const Vec3f& mc2 = m.getColumn(2); v_[0].setValue(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)); v_[1].setValue(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)); v_[2].setValue(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2)); return *this; } IMatrix3& IMatrix3::operator *= (const IMatrix3& m) { const IVector3& mc0 = m.getColumn(0); const IVector3& mc1 = m.getColumn(1); const IVector3& mc2 = m.getColumn(2); v_[0].setValue(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)); v_[1].setValue(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)); v_[2].setValue(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2)); return *this; } IMatrix3 IMatrix3::operator + (const IMatrix3& m) const { return IMatrix3(v_[0] + m.v_[0], v_[1] + m.v_[1], v_[2] + m.v_[2]); } IMatrix3& IMatrix3::operator += (const IMatrix3& m) { v_[0] += m.v_[0]; v_[1] += m.v_[1]; v_[2] += m.v_[2]; return *this; } IMatrix3 IMatrix3::operator - (const IMatrix3& m) const { return IMatrix3(v_[0] - m.v_[0], v_[1] - m.v_[1], v_[2] - m.v_[2]); } IMatrix3& IMatrix3::operator -= (const IMatrix3& m) { v_[0] -= m.v_[0]; v_[1] -= m.v_[1]; v_[2] -= m.v_[2]; return *this; } IMatrix3& IMatrix3::rotationConstrain() { for(std::size_t i = 0; i < 3; ++i) { for(std::size_t j = 0; j < 3; ++j) { if(v_[i][j][0] < -1) v_[i][j][0] = -1; else if(v_[i][j][0] > 1) v_[i][j][0] = 1; if(v_[i][j][1] < -1) v_[i][j][1] = -1; else if(v_[i][j][1] > 1) v_[i][j][1] = 1; } } return *this; } void IMatrix3::print() const { std::cout << "[" << v_[0][0][0] << "," << v_[0][0][1] << "]" << " [" << v_[0][1][0] << "," << v_[0][1][1] << "]" << " [" << v_[0][2][0] << "," << v_[0][2][1] << "]" << std::endl; std::cout << "[" << v_[1][0][0] << "," << v_[1][0][1] << "]" << " [" << v_[1][1][0] << "," << v_[1][1][1] << "]" << " [" << v_[1][2][0] << "," << v_[1][2][1] << "]" << std::endl; std::cout << "[" << v_[2][0][0] << "," << v_[2][0][1] << "]" << " [" << v_[2][1][0] << "," << v_[2][1][1] << "]" << " [" << v_[2][2][0] << "," << v_[2][2][1] << "]" << std::endl; } IMatrix3 rotationConstrain(const IMatrix3& m) { IMatrix3 res; for(std::size_t i = 0; i < 3; ++i) { for(std::size_t j = 0; j < 3; ++j) { if(m(i, j)[0] < -1) res(i, j)[0] = -1; else if(m(i, j)[0] > 1) res(i, j)[0] = 1; if(m(i, j)[1] < -1) res(i, j)[1] = -1; else if(m(i, j)[1] > 1) res(i, j)[1] = 1; } } return res; } } fcl-0.5.0/src/ccd/interval_vector.cpp000066400000000000000000000140571274356571000175330ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/ccd/interval_vector.h" #include namespace fcl { IVector3::IVector3() {} IVector3::IVector3(FCL_REAL v) { setValue(v); } IVector3::IVector3(FCL_REAL x, FCL_REAL y, FCL_REAL z) { setValue(x, y, z); } IVector3::IVector3(FCL_REAL xl, FCL_REAL xu, FCL_REAL yl, FCL_REAL yu, FCL_REAL zl, FCL_REAL zu) { setValue(xl, xu, yl, yu, zl, zu); } IVector3::IVector3(FCL_REAL v[3][2]) { setValue(v); } IVector3::IVector3(Interval v[3]) { setValue(v); } IVector3::IVector3(const Interval& v1, const Interval& v2, const Interval& v3) { setValue(v1, v2, v3); } IVector3::IVector3(const Vec3f& v) { setValue(v); } void IVector3::setZero() { setValue((FCL_REAL)0.0); } IVector3 IVector3::operator + (const IVector3& other) const { return IVector3(i_[0] + other.i_[0], i_[1] + other.i_[1], i_[2] + other.i_[2]); } IVector3& IVector3::operator += (const IVector3& other) { i_[0] += other[0]; i_[1] += other[1]; i_[2] += other[2]; return *this; } IVector3 IVector3::operator - (const IVector3& other) const { return IVector3(i_[0] - other.i_[0], i_[1] - other.i_[1], i_[2] - other.i_[2]); } IVector3& IVector3::operator -= (const IVector3& other) { i_[0] -= other[0]; i_[1] -= other[1]; i_[2] -= other[2]; return *this; } Interval IVector3::dot(const IVector3& other) const { return i_[0] * other.i_[0] + i_[1] * other.i_[1] + i_[2] * other.i_[2]; } IVector3 IVector3::cross(const IVector3& other) const { return IVector3(i_[1] * other.i_[2] - i_[2] * other.i_[1], i_[2] * other.i_[0] - i_[0] * other.i_[2], i_[0] * other.i_[1] - i_[1] * other.i_[0]); } Interval IVector3::dot(const Vec3f& other) const { return i_[0] * other[0] + i_[1] * other[1] + i_[2] * other[2]; } IVector3 IVector3::cross(const Vec3f& other) const { return IVector3(i_[1] * other[2] - i_[2] * other[1], i_[2] * other[0] - i_[0] * other[2], i_[0] * other[1] - i_[1] * other[0]); } FCL_REAL IVector3::volumn() const { return i_[0].diameter() * i_[1].diameter() * i_[2].diameter(); } void IVector3::print() const { std::cout << "[" << i_[0][0] << "," << i_[0][1] << "]" << std::endl; std::cout << "[" << i_[1][0] << "," << i_[1][1] << "]" << std::endl; std::cout << "[" << i_[2][0] << "," << i_[2][1] << "]" << std::endl; } Vec3f IVector3::center() const { return Vec3f(i_[0].center(), i_[1].center(), i_[2].center()); } void IVector3::bound(const IVector3& v) { if(v[0][0] < i_[0][0]) i_[0][0] = v[0][0]; if(v[1][0] < i_[1][0]) i_[1][0] = v[1][0]; if(v[2][0] < i_[2][0]) i_[2][0] = v[2][0]; if(v[0][1] > i_[0][1]) i_[0][1] = v[0][1]; if(v[1][1] > i_[1][1]) i_[1][1] = v[1][1]; if(v[2][1] > i_[2][1]) i_[2][1] = v[2][1]; } void IVector3::bound(const Vec3f& v) { if(v[0] < i_[0][0]) i_[0][0] = v[0]; if(v[1] < i_[1][0]) i_[1][0] = v[1]; if(v[2] < i_[2][0]) i_[2][0] = v[2]; if(v[0] > i_[0][1]) i_[0][1] = v[0]; if(v[1] > i_[1][1]) i_[1][1] = v[1]; if(v[2] > i_[2][1]) i_[2][1] = v[2]; } IVector3 bound(const IVector3& i, const IVector3& v) { IVector3 res(i); if(v[0][0] < res.i_[0][0]) res.i_[0][0] = v[0][0]; if(v[1][0] < res.i_[1][0]) res.i_[1][0] = v[1][0]; if(v[2][0] < res.i_[2][0]) res.i_[2][0] = v[2][0]; if(v[0][1] > res.i_[0][1]) res.i_[0][1] = v[0][1]; if(v[1][1] > res.i_[1][1]) res.i_[1][1] = v[1][1]; if(v[2][1] > res.i_[2][1]) res.i_[2][1] = v[2][1]; return res; } IVector3 bound(const IVector3& i, const Vec3f& v) { IVector3 res(i); if(v[0] < res.i_[0][0]) res.i_[0][0] = v[0]; if(v[1] < res.i_[1][0]) res.i_[1][0] = v[1]; if(v[2] < res.i_[2][0]) res.i_[2][0] = v[2]; if(v[0] > res.i_[0][1]) res.i_[0][1] = v[0]; if(v[1] > res.i_[1][1]) res.i_[1][1] = v[1]; if(v[2] > res.i_[2][1]) res.i_[2][1] = v[2]; return res; } bool IVector3::overlap(const IVector3& v) const { if(v[0][1] < i_[0][0]) return false; if(v[1][1] < i_[1][0]) return false; if(v[2][1] < i_[2][0]) return false; if(v[0][0] > i_[0][1]) return false; if(v[1][0] > i_[1][1]) return false; if(v[2][0] > i_[2][1]) return false; return true; } bool IVector3::contain(const IVector3& v) const { if(v[0][0] < i_[0][0]) return false; if(v[1][0] < i_[1][0]) return false; if(v[2][0] < i_[2][0]) return false; if(v[0][1] > i_[0][1]) return false; if(v[1][1] > i_[1][1]) return false; if(v[2][1] > i_[2][1]) return false; return true; } } fcl-0.5.0/src/ccd/motion.cpp000066400000000000000000000435421274356571000156330ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/ccd/motion.h" namespace fcl { template<> FCL_REAL TBVMotionBoundVisitor::visit(const SplineMotion& motion) const { FCL_REAL T_bound = motion.computeTBound(n); FCL_REAL tf_t = motion.getCurrentTime(); Vec3f c1 = bv.Tr; Vec3f c2 = bv.Tr + bv.axis[0] * bv.l[0]; Vec3f c3 = bv.Tr + bv.axis[1] * bv.l[1]; Vec3f c4 = bv.Tr + bv.axis[0] * bv.l[0] + bv.axis[1] * bv.l[1]; FCL_REAL tmp; // max_i |c_i * n| FCL_REAL cn_max = std::abs(c1.dot(n)); tmp = std::abs(c2.dot(n)); if(tmp > cn_max) cn_max = tmp; tmp = std::abs(c3.dot(n)); if(tmp > cn_max) cn_max = tmp; tmp = std::abs(c4.dot(n)); if(tmp > cn_max) cn_max = tmp; // max_i ||c_i|| FCL_REAL cmax = c1.sqrLength(); tmp = c2.sqrLength(); if(tmp > cmax) cmax = tmp; tmp = c3.sqrLength(); if(tmp > cmax) cmax = tmp; tmp = c4.sqrLength(); if(tmp > cmax) cmax = tmp; cmax = sqrt(cmax); // max_i ||c_i x n|| FCL_REAL cxn_max = (c1.cross(n)).sqrLength(); tmp = (c2.cross(n)).sqrLength(); if(tmp > cxn_max) cxn_max = tmp; tmp = (c3.cross(n)).sqrLength(); if(tmp > cxn_max) cxn_max = tmp; tmp = (c4.cross(n)).sqrLength(); if(tmp > cxn_max) cxn_max = tmp; cxn_max = sqrt(cxn_max); FCL_REAL dWdW_max = motion.computeDWMax(); FCL_REAL ratio = std::min(1 - tf_t, dWdW_max); FCL_REAL R_bound = 2 * (cn_max + cmax + cxn_max + 3 * bv.r) * ratio; // std::cout << R_bound << " " << T_bound << std::endl; return R_bound + T_bound; } FCL_REAL TriangleMotionBoundVisitor::visit(const SplineMotion& motion) const { FCL_REAL T_bound = motion.computeTBound(n); FCL_REAL tf_t = motion.getCurrentTime(); FCL_REAL R_bound = std::abs(a.dot(n)) + a.length() + (a.cross(n)).length(); FCL_REAL R_bound_tmp = std::abs(b.dot(n)) + b.length() + (b.cross(n)).length(); if(R_bound_tmp > R_bound) R_bound = R_bound_tmp; R_bound_tmp = std::abs(c.dot(n)) + c.length() + (c.cross(n)).length(); if(R_bound_tmp > R_bound) R_bound = R_bound_tmp; FCL_REAL dWdW_max = motion.computeDWMax(); FCL_REAL ratio = std::min(1 - tf_t, dWdW_max); R_bound *= 2 * ratio; // std::cout << R_bound << " " << T_bound << std::endl; return R_bound + T_bound; } SplineMotion::SplineMotion(const Vec3f& Td0, const Vec3f& Td1, const Vec3f& Td2, const Vec3f& Td3, const Vec3f& Rd0, const Vec3f& Rd1, const Vec3f& Rd2, const Vec3f& Rd3) : MotionBase() { Td[0] = Td0; Td[1] = Td1; Td[2] = Td2; Td[3] = Td3; Rd[0] = Rd0; Rd[1] = Rd1; Rd[2] = Rd2; Rd[3] = Rd3; Rd0Rd0 = Rd[0].dot(Rd[0]); Rd0Rd1 = Rd[0].dot(Rd[1]); Rd0Rd2 = Rd[0].dot(Rd[2]); Rd0Rd3 = Rd[0].dot(Rd[3]); Rd1Rd1 = Rd[1].dot(Rd[1]); Rd1Rd2 = Rd[1].dot(Rd[2]); Rd1Rd3 = Rd[1].dot(Rd[3]); Rd2Rd2 = Rd[2].dot(Rd[2]); Rd2Rd3 = Rd[2].dot(Rd[3]); Rd3Rd3 = Rd[3].dot(Rd[3]); TA = Td[1] * 3 - Td[2] * 3 + Td[3] - Td[0]; TB = (Td[0] - Td[1] * 2 + Td[2]) * 3; TC = (Td[2] - Td[0]) * 3; RA = Rd[1] * 3 - Rd[2] * 3 + Rd[3] - Rd[0]; RB = (Rd[0] - Rd[1] * 2 + Rd[2]) * 3; RC = (Rd[2] - Rd[0]) * 3; integrate(0.0); } bool SplineMotion::integrate(double dt) const { if(dt > 1) dt = 1; Vec3f cur_T = Td[0] * getWeight0(dt) + Td[1] * getWeight1(dt) + Td[2] * getWeight2(dt) + Td[3] * getWeight3(dt); Vec3f cur_w = Rd[0] * getWeight0(dt) + Rd[1] * getWeight1(dt) + Rd[2] * getWeight2(dt) + Rd[3] * getWeight3(dt); FCL_REAL cur_angle = cur_w.length(); cur_w.normalize(); Quaternion3f cur_q; cur_q.fromAxisAngle(cur_w, cur_angle); tf.setTransform(cur_q, cur_T); tf_t = dt; return true; } FCL_REAL SplineMotion::computeTBound(const Vec3f& n) const { FCL_REAL Ta = TA.dot(n); FCL_REAL Tb = TB.dot(n); FCL_REAL Tc = TC.dot(n); std::vector T_potential; T_potential.push_back(tf_t); T_potential.push_back(1); if(Tb * Tb - 3 * Ta * Tc >= 0) { if(Ta == 0) { if(Tb != 0) { FCL_REAL tmp = -Tc / (2 * Tb); if(tmp < 1 && tmp > tf_t) T_potential.push_back(tmp); } } else { FCL_REAL tmp_delta = sqrt(Tb * Tb - 3 * Ta * Tc); FCL_REAL tmp1 = (-Tb + tmp_delta) / (3 * Ta); FCL_REAL tmp2 = (-Tb - tmp_delta) / (3 * Ta); if(tmp1 < 1 && tmp1 > tf_t) T_potential.push_back(tmp1); if(tmp2 < 1 && tmp2 > tf_t) T_potential.push_back(tmp2); } } FCL_REAL T_bound = Ta * T_potential[0] * T_potential[0] * T_potential[0] + Tb * T_potential[0] * T_potential[0] + Tc * T_potential[0]; for(unsigned int i = 1; i < T_potential.size(); ++i) { FCL_REAL T_bound_tmp = Ta * T_potential[i] * T_potential[i] * T_potential[i] + Tb * T_potential[i] * T_potential[i] + Tc * T_potential[i]; if(T_bound_tmp > T_bound) T_bound = T_bound_tmp; } FCL_REAL cur_delta = Ta * tf_t * tf_t * tf_t + Tb * tf_t * tf_t + Tc * tf_t; T_bound -= cur_delta; T_bound /= 6.0; return T_bound; } FCL_REAL SplineMotion::computeDWMax() const { // first compute ||w'|| int a00[5] = {1,-4,6,-4,1}; int a01[5] = {-3,10,-11,4,0}; int a02[5] = {3,-8,6,0,-1}; int a03[5] = {-1,2,-1,0,0}; int a11[5] = {9,-24,16,0,0}; int a12[5] = {-9,18,-5,-4,0}; int a13[5] = {3,-4,0,0,0}; int a22[5] = {9,-12,-2,4,1}; int a23[5] = {-3,2,1,0,0}; int a33[5] = {1,0,0,0,0}; FCL_REAL a[5]; for(int i = 0; i < 5; ++i) { a[i] = Rd0Rd0 * a00[i] + Rd0Rd1 * a01[i] + Rd0Rd2 * a02[i] + Rd0Rd3 * a03[i] + Rd0Rd1 * a01[i] + Rd1Rd1 * a11[i] + Rd1Rd2 * a12[i] + Rd1Rd3 * a13[i] + Rd0Rd2 * a02[i] + Rd1Rd2 * a12[i] + Rd2Rd2 * a22[i] + Rd2Rd3 * a23[i] + Rd0Rd3 * a03[i] + Rd1Rd3 * a13[i] + Rd2Rd3 * a23[i] + Rd3Rd3 * a33[i]; a[i] /= 4.0; } // compute polynomial for ||w'||' int da00[4] = {4,-12,12,-4}; int da01[4] = {-12,30,-22,4}; int da02[4] = {12,-24,12,0}; int da03[4] = {-4,6,-2,0}; int da11[4] = {36,-72,32,0}; int da12[4] = {-36,54,-10,-4}; int da13[4] = {12,-12,0,0}; int da22[4] = {36,-36,-4,4}; int da23[4] = {-12,6,2,0}; int da33[4] = {4,0,0,0}; FCL_REAL da[4]; for(int i = 0; i < 4; ++i) { da[i] = Rd0Rd0 * da00[i] + Rd0Rd1 * da01[i] + Rd0Rd2 * da02[i] + Rd0Rd3 * da03[i] + Rd0Rd1 * da01[i] + Rd1Rd1 * da11[i] + Rd1Rd2 * da12[i] + Rd1Rd3 * da13[i] + Rd0Rd2 * da02[i] + Rd1Rd2 * da12[i] + Rd2Rd2 * da22[i] + Rd2Rd3 * da23[i] + Rd0Rd3 * da03[i] + Rd1Rd3 * da13[i] + Rd2Rd3 * da23[i] + Rd3Rd3 * da33[i]; da[i] /= 4.0; } FCL_REAL roots[3]; int root_num = PolySolver::solveCubic(da, roots); FCL_REAL dWdW_max = a[0] * tf_t * tf_t * tf_t + a[1] * tf_t * tf_t * tf_t + a[2] * tf_t * tf_t + a[3] * tf_t + a[4]; FCL_REAL dWdW_1 = a[0] + a[1] + a[2] + a[3] + a[4]; if(dWdW_max < dWdW_1) dWdW_max = dWdW_1; for(int i = 0; i < root_num; ++i) { FCL_REAL v = roots[i]; if(v >= tf_t && v <= 1) { FCL_REAL value = a[0] * v * v * v * v + a[1] * v * v * v + a[2] * v * v + a[3] * v + a[4]; if(value > dWdW_max) dWdW_max = value; } } return sqrt(dWdW_max); } FCL_REAL SplineMotion::getWeight0(FCL_REAL t) const { return (1 - 3 * t + 3 * t * t - t * t * t) / 6.0; } FCL_REAL SplineMotion::getWeight1(FCL_REAL t) const { return (4 - 6 * t * t + 3 * t * t * t) / 6.0; } FCL_REAL SplineMotion::getWeight2(FCL_REAL t) const { return (1 + 3 * t + 3 * t * t - 3 * t * t * t) / 6.0; } FCL_REAL SplineMotion::getWeight3(FCL_REAL t) const { return t * t * t / 6.0; } /// @brief Compute the motion bound for a bounding volume along a given direction n /// according to mu < |v * n| + ||w x n||(r + max(||ci*||)) where ||ci*|| = ||R0(ci) x w||. w is the angular axis (normalized) /// and ci are the endpoints of the generator primitives of RSS. /// Notice that all bv parameters are in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame) template<> FCL_REAL TBVMotionBoundVisitor::visit(const ScrewMotion& motion) const { Transform3f tf; motion.getCurrentTransform(tf); const Vec3f& axis = motion.getAxis(); FCL_REAL linear_vel = motion.getLinearVelocity(); FCL_REAL angular_vel = motion.getAngularVelocity(); const Vec3f& p = motion.getAxisOrigin(); FCL_REAL c_proj_max = ((tf.getQuatRotation().transform(bv.Tr)).cross(axis)).sqrLength(); FCL_REAL tmp; tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0])).cross(axis)).sqrLength(); if(tmp > c_proj_max) c_proj_max = tmp; tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[1] * bv.l[1])).cross(axis)).sqrLength(); if(tmp > c_proj_max) c_proj_max = tmp; tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0] + bv.axis[1] * bv.l[1])).cross(axis)).sqrLength(); if(tmp > c_proj_max) c_proj_max = tmp; c_proj_max = sqrt(c_proj_max); FCL_REAL v_dot_n = axis.dot(n) * linear_vel; FCL_REAL w_cross_n = (axis.cross(n)).length() * angular_vel; FCL_REAL origin_proj = ((tf.getTranslation() - p).cross(axis)).length(); FCL_REAL mu = v_dot_n + w_cross_n * (c_proj_max + bv.r + origin_proj); return mu; } FCL_REAL TriangleMotionBoundVisitor::visit(const ScrewMotion& motion) const { Transform3f tf; motion.getCurrentTransform(tf); const Vec3f& axis = motion.getAxis(); FCL_REAL linear_vel = motion.getLinearVelocity(); FCL_REAL angular_vel = motion.getAngularVelocity(); const Vec3f& p = motion.getAxisOrigin(); FCL_REAL proj_max = ((tf.getQuatRotation().transform(a) + tf.getTranslation() - p).cross(axis)).sqrLength(); FCL_REAL tmp; tmp = ((tf.getQuatRotation().transform(b) + tf.getTranslation() - p).cross(axis)).sqrLength(); if(tmp > proj_max) proj_max = tmp; tmp = ((tf.getQuatRotation().transform(c) + tf.getTranslation() - p).cross(axis)).sqrLength(); if(tmp > proj_max) proj_max = tmp; proj_max = std::sqrt(proj_max); FCL_REAL v_dot_n = axis.dot(n) * linear_vel; FCL_REAL w_cross_n = (axis.cross(n)).length() * angular_vel; FCL_REAL mu = v_dot_n + w_cross_n * proj_max; return mu; } /// @brief Compute the motion bound for a bounding volume along a given direction n /// according to mu < |v * n| + ||w x n||(r + max(||ci*||)) where ||ci*|| = ||R0(ci) x w||. w is the angular axis (normalized) /// and ci are the endpoints of the generator primitives of RSS. /// Notice that all bv parameters are in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame) template<> FCL_REAL TBVMotionBoundVisitor::visit(const InterpMotion& motion) const { Transform3f tf; motion.getCurrentTransform(tf); const Vec3f& reference_p = motion.getReferencePoint(); const Vec3f& angular_axis = motion.getAngularAxis(); FCL_REAL angular_vel = motion.getAngularVelocity(); const Vec3f& linear_vel = motion.getLinearVelocity(); FCL_REAL c_proj_max = ((tf.getQuatRotation().transform(bv.Tr - reference_p)).cross(angular_axis)).sqrLength(); FCL_REAL tmp; tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0] - reference_p)).cross(angular_axis)).sqrLength(); if(tmp > c_proj_max) c_proj_max = tmp; tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[1] * bv.l[1] - reference_p)).cross(angular_axis)).sqrLength(); if(tmp > c_proj_max) c_proj_max = tmp; tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0] + bv.axis[1] * bv.l[1] - reference_p)).cross(angular_axis)).sqrLength(); if(tmp > c_proj_max) c_proj_max = tmp; c_proj_max = std::sqrt(c_proj_max); FCL_REAL v_dot_n = linear_vel.dot(n); FCL_REAL w_cross_n = (angular_axis.cross(n)).length() * angular_vel; FCL_REAL mu = v_dot_n + w_cross_n * (bv.r + c_proj_max); return mu; } /// @brief Compute the motion bound for a triangle along a given direction n /// according to mu < |v * n| + ||w x n||(max||ci*||) where ||ci*|| = ||R0(ci) x w|| / \|w\|. w is the angular velocity /// and ci are the triangle vertex coordinates. /// Notice that the triangle is in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame) FCL_REAL TriangleMotionBoundVisitor::visit(const InterpMotion& motion) const { Transform3f tf; motion.getCurrentTransform(tf); const Vec3f& reference_p = motion.getReferencePoint(); const Vec3f& angular_axis = motion.getAngularAxis(); FCL_REAL angular_vel = motion.getAngularVelocity(); const Vec3f& linear_vel = motion.getLinearVelocity(); FCL_REAL proj_max = ((tf.getQuatRotation().transform(a - reference_p)).cross(angular_axis)).sqrLength(); FCL_REAL tmp; tmp = ((tf.getQuatRotation().transform(b - reference_p)).cross(angular_axis)).sqrLength(); if(tmp > proj_max) proj_max = tmp; tmp = ((tf.getQuatRotation().transform(c - reference_p)).cross(angular_axis)).sqrLength(); if(tmp > proj_max) proj_max = tmp; proj_max = std::sqrt(proj_max); FCL_REAL v_dot_n = linear_vel.dot(n); FCL_REAL w_cross_n = (angular_axis.cross(n)).length() * angular_vel; FCL_REAL mu = v_dot_n + w_cross_n * proj_max; return mu; } InterpMotion::InterpMotion() : MotionBase() { // Default angular velocity is zero angular_axis.setValue(1, 0, 0); angular_vel = 0; // Default reference point is local zero point // Default linear velocity is zero } InterpMotion::InterpMotion(const Matrix3f& R1, const Vec3f& T1, const Matrix3f& R2, const Vec3f& T2) : MotionBase(), tf1(R1, T1), tf2(R2, T2), tf(tf1) { // Compute the velocities for the motion computeVelocity(); } InterpMotion::InterpMotion(const Transform3f& tf1_, const Transform3f& tf2_) : MotionBase(), tf1(tf1_), tf2(tf2_), tf(tf1) { // Compute the velocities for the motion computeVelocity(); } InterpMotion::InterpMotion(const Matrix3f& R1, const Vec3f& T1, const Matrix3f& R2, const Vec3f& T2, const Vec3f& O) : MotionBase(), tf1(R1, T1), tf2(R2, T2), tf(tf1), reference_p(O) { // Compute the velocities for the motion computeVelocity(); } InterpMotion::InterpMotion(const Transform3f& tf1_, const Transform3f& tf2_, const Vec3f& O) : MotionBase(), tf1(tf1_), tf2(tf2_), tf(tf1), reference_p(O) { } bool InterpMotion::integrate(double dt) const { if(dt > 1) dt = 1; tf.setQuatRotation(absoluteRotation(dt)); tf.setTranslation(linear_vel * dt + tf1.transform(reference_p) - tf.getQuatRotation().transform(reference_p)); return true; } void InterpMotion::computeVelocity() { linear_vel = tf2.transform(reference_p) - tf1.transform(reference_p); Quaternion3f deltaq = tf2.getQuatRotation() * inverse(tf1.getQuatRotation()); deltaq.toAxisAngle(angular_axis, angular_vel); if(angular_vel < 0) { angular_vel = -angular_vel; angular_axis = -angular_axis; } } Quaternion3f InterpMotion::deltaRotation(FCL_REAL dt) const { Quaternion3f res; res.fromAxisAngle(angular_axis, (FCL_REAL)(dt * angular_vel)); return res; } Quaternion3f InterpMotion::absoluteRotation(FCL_REAL dt) const { Quaternion3f delta_t = deltaRotation(dt); return delta_t * tf1.getQuatRotation(); } /// @brief Compute the motion bound for a bounding volume along a given direction n template<> FCL_REAL TBVMotionBoundVisitor::visit(const TranslationMotion& motion) const { return motion.getVelocity().dot(n); } /// @brief Compute the motion bound for a triangle along a given direction n FCL_REAL TriangleMotionBoundVisitor::visit(const TranslationMotion& motion) const { return motion.getVelocity().dot(n); } template class TBVMotionBoundVisitor; } fcl-0.5.0/src/ccd/taylor_matrix.cpp000066400000000000000000000244631274356571000172250ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/ccd/taylor_matrix.h" namespace fcl { TMatrix3::TMatrix3() { } TMatrix3::TMatrix3(const std::shared_ptr& time_interval) { setTimeInterval(time_interval); } TMatrix3::TMatrix3(TaylorModel m[3][3]) { v_[0] = TVector3(m[0]); v_[1] = TVector3(m[1]); v_[2] = TVector3(m[2]); } TMatrix3::TMatrix3(const TVector3& v1, const TVector3& v2, const TVector3& v3) { v_[0] = v1; v_[1] = v2; v_[2] = v3; } TMatrix3::TMatrix3(const Matrix3f& m, const std::shared_ptr& time_interval) { v_[0] = TVector3(m.getRow(0), time_interval); v_[1] = TVector3(m.getRow(1), time_interval); v_[2] = TVector3(m.getRow(2), time_interval); } void TMatrix3::setIdentity() { setZero(); v_[0][0].coeff(0) = 1; v_[1][1].coeff(0) = 1; v_[2][2].coeff(0) = 1; } void TMatrix3::setZero() { v_[0].setZero(); v_[1].setZero(); v_[2].setZero(); } TVector3 TMatrix3::getColumn(size_t i) const { return TVector3(v_[0][i], v_[1][i], v_[2][i]); } const TVector3& TMatrix3::getRow(size_t i) const { return v_[i]; } const TaylorModel& TMatrix3::operator () (size_t i, size_t j) const { return v_[i][j]; } TaylorModel& TMatrix3::operator () (size_t i, size_t j) { return v_[i][j]; } TMatrix3 TMatrix3::operator * (const Matrix3f& m) const { const Vec3f& mc0 = m.getColumn(0); const Vec3f& mc1 = m.getColumn(1); const Vec3f& mc2 = m.getColumn(2); return TMatrix3(TVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)), TVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)), TVector3(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2))); } TVector3 TMatrix3::operator * (const Vec3f& v) const { return TVector3(v_[0].dot(v), v_[1].dot(v), v_[2].dot(v)); } TVector3 TMatrix3::operator * (const TVector3& v) const { return TVector3(v_[0].dot(v), v_[1].dot(v), v_[2].dot(v)); } TMatrix3 TMatrix3::operator * (const TMatrix3& m) const { const TVector3& mc0 = m.getColumn(0); const TVector3& mc1 = m.getColumn(1); const TVector3& mc2 = m.getColumn(2); return TMatrix3(TVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)), TVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)), TVector3(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2))); } TMatrix3 TMatrix3::operator * (const TaylorModel& d) const { return TMatrix3(v_[0] * d, v_[1] * d, v_[2] * d); } TMatrix3 TMatrix3::operator * (FCL_REAL d) const { return TMatrix3(v_[0] * d, v_[1] * d, v_[2] * d); } TMatrix3& TMatrix3::operator *= (const Matrix3f& m) { const Vec3f& mc0 = m.getColumn(0); const Vec3f& mc1 = m.getColumn(1); const Vec3f& mc2 = m.getColumn(2); v_[0] = TVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)); v_[1] = TVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)); v_[2] = TVector3(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2)); return *this; } TMatrix3& TMatrix3::operator *= (const TMatrix3& m) { const TVector3& mc0 = m.getColumn(0); const TVector3& mc1 = m.getColumn(1); const TVector3& mc2 = m.getColumn(2); v_[0] = TVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)); v_[1] = TVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)); v_[2] = TVector3(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2)); return *this; } TMatrix3& TMatrix3::operator *= (const TaylorModel& d) { v_[0] *= d; v_[1] *= d; v_[2] *= d; return *this; } TMatrix3& TMatrix3::operator *= (FCL_REAL d) { v_[0] *= d; v_[1] *= d; v_[2] *= d; return *this; } TMatrix3 TMatrix3::operator + (const TMatrix3& m) const { return TMatrix3(v_[0] + m.v_[0], v_[1] + m.v_[1], v_[2] + m.v_[2]); } TMatrix3& TMatrix3::operator += (const TMatrix3& m) { v_[0] += m.v_[0]; v_[1] += m.v_[1]; v_[2] += m.v_[2]; return *this; } TMatrix3 TMatrix3::operator + (const Matrix3f& m) const { TMatrix3 res = *this; res += m; return res; } TMatrix3& TMatrix3::operator += (const Matrix3f& m) { for(std::size_t i = 0; i < 3; ++i) { for(std::size_t j = 0; j < 3; ++j) v_[i][j] += m(i, j); } return *this; } TMatrix3 TMatrix3::operator - (const TMatrix3& m) const { return TMatrix3(v_[0] - m.v_[0], v_[1] - m.v_[1], v_[2] - m.v_[2]); } TMatrix3& TMatrix3::operator -= (const TMatrix3& m) { v_[0] -= m.v_[0]; v_[1] -= m.v_[1]; v_[2] -= m.v_[2]; return *this; } TMatrix3 TMatrix3::operator - (const Matrix3f& m) const { TMatrix3 res = *this; res -= m; return res; } TMatrix3& TMatrix3::operator -= (const Matrix3f& m) { for(std::size_t i = 0; i < 3; ++i) { for(std::size_t j = 0; j < 3; ++j) v_[i][j] -= m(i, j); } return *this; } TMatrix3 TMatrix3::operator - () const { return TMatrix3(-v_[0], -v_[1], -v_[2]); } void TMatrix3::print() const { getColumn(0).print(); getColumn(1).print(); getColumn(2).print(); } IMatrix3 TMatrix3::getBound() const { return IMatrix3(v_[0].getBound(), v_[1].getBound(), v_[2].getBound()); } IMatrix3 TMatrix3::getBound(FCL_REAL l, FCL_REAL r) const { return IMatrix3(v_[0].getBound(l, r), v_[1].getBound(l, r), v_[2].getBound(l, r)); } IMatrix3 TMatrix3::getBound(FCL_REAL t) const { return IMatrix3(v_[0].getBound(t), v_[1].getBound(t), v_[2].getBound(t)); } IMatrix3 TMatrix3::getTightBound() const { return IMatrix3(v_[0].getTightBound(), v_[1].getTightBound(), v_[2].getTightBound()); } IMatrix3 TMatrix3::getTightBound(FCL_REAL l, FCL_REAL r) const { return IMatrix3(v_[0].getTightBound(l, r), v_[1].getTightBound(l, r), v_[2].getTightBound(l, r)); } FCL_REAL TMatrix3::diameter() const { FCL_REAL d = 0; FCL_REAL tmp = v_[0][0].remainder().diameter(); if(tmp > d) d = tmp; tmp = v_[0][1].remainder().diameter(); if(tmp > d) d = tmp; tmp = v_[0][2].remainder().diameter(); if(tmp > d) d = tmp; tmp = v_[1][0].remainder().diameter(); if(tmp > d) d = tmp; tmp = v_[1][1].remainder().diameter(); if(tmp > d) d = tmp; tmp = v_[1][2].remainder().diameter(); if(tmp > d) d = tmp; tmp = v_[2][0].remainder().diameter(); if(tmp > d) d = tmp; tmp = v_[2][1].remainder().diameter(); if(tmp > d) d = tmp; tmp = v_[2][2].remainder().diameter(); if(tmp > d) d = tmp; return d; } void TMatrix3::setTimeInterval(const std::shared_ptr& time_interval) { v_[0].setTimeInterval(time_interval); v_[1].setTimeInterval(time_interval); v_[2].setTimeInterval(time_interval); } void TMatrix3::setTimeInterval(FCL_REAL l, FCL_REAL r) { v_[0].setTimeInterval(l, r); v_[1].setTimeInterval(l, r); v_[2].setTimeInterval(l, r); } const std::shared_ptr& TMatrix3::getTimeInterval() const { return v_[0].getTimeInterval(); } TMatrix3& TMatrix3::rotationConstrain() { for(std::size_t i = 0; i < 3; ++i) { for(std::size_t j = 0; j < 3; ++j) { if(v_[i][j].remainder()[0] < -1) v_[i][j].remainder()[0] = -1; else if(v_[i][j].remainder()[0] > 1) v_[i][j].remainder()[0] = 1; if(v_[i][j].remainder()[1] < -1) v_[i][j].remainder()[1] = -1; else if(v_[i][j].remainder()[1] > 1) v_[i][j].remainder()[1] = 1; if((v_[i][j].remainder()[0] == -1) && (v_[i][j].remainder()[1] == 1)) { v_[i][j].coeff(0) = 0; v_[i][j].coeff(1) = 0; v_[i][j].coeff(2) = 0; v_[i][j].coeff(3) = 0; } } } return *this; } TMatrix3 rotationConstrain(const TMatrix3& m) { TMatrix3 res; for(std::size_t i = 0; i < 3; ++i) { for(std::size_t j = 0; j < 3; ++j) { if(m(i, j).remainder()[0] < -1) res(i, j).remainder()[0] = -1; else if(m(i, j).remainder()[0] > 1) res(i, j).remainder()[0] = 1; if(m(i, j).remainder()[1] < -1) res(i, j).remainder()[1] = -1; else if(m(i, j).remainder()[1] > 1) res(i, j).remainder()[1] = 1; if((m(i, j).remainder()[0] == -1) && (m(i, j).remainder()[1] == 1)) { res(i, j).coeff(0) = 0; res(i, j).coeff(1) = 0; res(i, j).coeff(2) = 0; res(i, j).coeff(3) = 0; } } } return res; } TMatrix3 operator * (const Matrix3f& m, const TaylorModel& a) { TMatrix3 res(a.getTimeInterval()); res(0, 0) = a * m(0, 0); res(0, 1) = a * m(0, 1); res(0, 1) = a * m(0, 2); res(1, 0) = a * m(1, 0); res(1, 1) = a * m(1, 1); res(1, 1) = a * m(1, 2); res(2, 0) = a * m(2, 0); res(2, 1) = a * m(2, 1); res(2, 1) = a * m(2, 2); return res; } TMatrix3 operator * (const TaylorModel& a, const Matrix3f& m) { return m * a; } TMatrix3 operator * (const TaylorModel& a, const TMatrix3& m) { return m * a; } TMatrix3 operator * (FCL_REAL d, const TMatrix3& m) { return m * d; } TMatrix3 operator + (const Matrix3f& m1, const TMatrix3& m2) { return m2 + m1; } TMatrix3 operator - (const Matrix3f& m1, const TMatrix3& m2) { return -m2 + m1; } } fcl-0.5.0/src/ccd/taylor_model.cpp000066400000000000000000000353401274356571000170150ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/math/constants.h" #include "fcl/ccd/taylor_model.h" #include #include #include namespace fcl { TaylorModel::TaylorModel() { coeffs_[0] = coeffs_[1] = coeffs_[2] = coeffs_[3] = 0; } TaylorModel::TaylorModel(const std::shared_ptr& time_interval) : time_interval_(time_interval) { coeffs_[0] = coeffs_[1] = coeffs_[2] = coeffs_[3] = 0; } TaylorModel::TaylorModel(FCL_REAL coeff, const std::shared_ptr& time_interval) : time_interval_(time_interval) { coeffs_[0] = coeff; coeffs_[1] = coeffs_[2] = coeffs_[3] = r_[0] = r_[1] = 0; } TaylorModel::TaylorModel(FCL_REAL coeffs[3], const Interval& r, const std::shared_ptr& time_interval) : time_interval_(time_interval) { coeffs_[0] = coeffs[0]; coeffs_[1] = coeffs[1]; coeffs_[2] = coeffs[2]; coeffs_[3] = coeffs[3]; r_ = r; } TaylorModel::TaylorModel(FCL_REAL c0, FCL_REAL c1, FCL_REAL c2, FCL_REAL c3, const Interval& r, const std::shared_ptr& time_interval) : time_interval_(time_interval) { coeffs_[0] = c0; coeffs_[1] = c1; coeffs_[2] = c2; coeffs_[3] = c3; r_ = r; } TaylorModel TaylorModel::operator + (FCL_REAL d) const { return TaylorModel(coeffs_[0] + d, coeffs_[1], coeffs_[2], coeffs_[3], r_, time_interval_); } TaylorModel& TaylorModel::operator += (FCL_REAL d) { coeffs_[0] += d; return *this; } TaylorModel TaylorModel::operator - (FCL_REAL d) const { return TaylorModel(coeffs_[0] - d, coeffs_[1], coeffs_[2], coeffs_[3], r_, time_interval_); } TaylorModel& TaylorModel::operator -= (FCL_REAL d) { coeffs_[0] -= d; return *this; } TaylorModel TaylorModel::operator + (const TaylorModel& other) const { assert(other.time_interval_ == time_interval_); return TaylorModel(coeffs_[0] + other.coeffs_[0], coeffs_[1] + other.coeffs_[1], coeffs_[2] + other.coeffs_[2], coeffs_[3] + other.coeffs_[3], r_ + other.r_, time_interval_); } TaylorModel TaylorModel::operator - (const TaylorModel& other) const { assert(other.time_interval_ == time_interval_); return TaylorModel(coeffs_[0] - other.coeffs_[0], coeffs_[1] - other.coeffs_[1], coeffs_[2] - other.coeffs_[2], coeffs_[3] - other.coeffs_[3], r_ - other.r_, time_interval_); } TaylorModel& TaylorModel::operator += (const TaylorModel& other) { assert(other.time_interval_ == time_interval_); coeffs_[0] += other.coeffs_[0]; coeffs_[1] += other.coeffs_[1]; coeffs_[2] += other.coeffs_[2]; coeffs_[3] += other.coeffs_[3]; r_ += other.r_; return *this; } TaylorModel& TaylorModel::operator -= (const TaylorModel& other) { assert(other.time_interval_ == time_interval_); coeffs_[0] -= other.coeffs_[0]; coeffs_[1] -= other.coeffs_[1]; coeffs_[2] -= other.coeffs_[2]; coeffs_[3] -= other.coeffs_[3]; r_ -= other.r_; return *this; } /// @brief Taylor model multiplication: /// f(t) = c0+c1*t+c2*t^2+c3*t^3+[a,b] /// g(t) = c0'+c1'*t+c2'*t^2+c3'*t^2+[c,d] /// f(t)g(t)= c0c0'+ /// (c0c1'+c1c0')t+ /// (c0c2'+c1c1'+c2c0')t^2+ /// (c0c3'+c1c2'+c2c1'+c3c0')t^3+ /// [a,b][c,d]+ /// (c1c3'+c2c2'+c3c1')t^4+ /// (c2c3'+c3c2')t^5+ /// (c3c3')t^6+ /// (c0+c1*t+c2*t^2+c3*t^3)[c,d]+ /// (c0'+c1'*t+c2'*t^2+c3'*c^3)[a,b] TaylorModel TaylorModel::operator * (const TaylorModel& other) const { TaylorModel res(*this); res *= other; return res; } TaylorModel TaylorModel::operator * (FCL_REAL d) const { return TaylorModel(coeffs_[0] * d, coeffs_[1] * d, coeffs_[2] * d, coeffs_[3] * d, r_ * d, time_interval_); } TaylorModel& TaylorModel::operator *= (const TaylorModel& other) { assert(other.time_interval_ == time_interval_); register FCL_REAL c0, c1, c2, c3; register FCL_REAL c0b = other.coeffs_[0], c1b = other.coeffs_[1], c2b = other.coeffs_[2], c3b = other.coeffs_[3]; const Interval& rb = other.r_; c0 = coeffs_[0] * c0b; c1 = coeffs_[0] * c1b + coeffs_[1] * c0b; c2 = coeffs_[0] * c2b + coeffs_[1] * c1b + coeffs_[2] * c0b; c3 = coeffs_[0] * c3b + coeffs_[1] * c2b + coeffs_[2] * c1b + coeffs_[3] * c0b; Interval remainder(r_ * rb); register FCL_REAL tempVal = coeffs_[1] * c3b + coeffs_[2] * c2b + coeffs_[3] * c1b; remainder += time_interval_->t4_ * tempVal; tempVal = coeffs_[2] * c3b + coeffs_[3] * c2b; remainder += time_interval_->t5_ * tempVal; tempVal = coeffs_[3] * c3b; remainder += time_interval_->t6_ * tempVal; remainder += ((Interval(coeffs_[0]) + time_interval_->t_ * coeffs_[1] + time_interval_->t2_ * coeffs_[2] + time_interval_->t3_ * coeffs_[3]) * rb + (Interval(c0b) + time_interval_->t_ * c1b + time_interval_->t2_ * c2b + time_interval_->t3_ * c3b) * r_); coeffs_[0] = c0; coeffs_[1] = c1; coeffs_[2] = c2; coeffs_[3] = c3; r_ = remainder; return *this; } TaylorModel& TaylorModel::operator *= (FCL_REAL d) { coeffs_[0] *= d; coeffs_[1] *= d; coeffs_[2] *= d; coeffs_[3] *= d; r_ *= d; return *this; } TaylorModel TaylorModel::operator - () const { return TaylorModel(-coeffs_[0], -coeffs_[1], -coeffs_[2], -coeffs_[3], -r_, time_interval_); } void TaylorModel::print() const { std::cout << coeffs_[0] << "+" << coeffs_[1] << "*t+" << coeffs_[2] << "*t^2+" << coeffs_[3] << "*t^3+[" << r_[0] << "," << r_[1] << "]" << std::endl; } Interval TaylorModel::getBound(FCL_REAL t) const { return Interval(coeffs_[0] + t * (coeffs_[1] + t * (coeffs_[2] + t * coeffs_[3]))) + r_; } Interval TaylorModel::getBound(FCL_REAL t0, FCL_REAL t1) const { Interval t(t0, t1); Interval t2(t0 * t0, t1 * t1); Interval t3(t0 * t2[0], t1 * t2[1]); return Interval(coeffs_[0]) + t * coeffs_[1] + t2 * coeffs_[2] + t3 * coeffs_[3] + r_; } Interval TaylorModel::getBound() const { return Interval(coeffs_[0] + r_[0], coeffs_[1] + r_[1]) + time_interval_->t_ * coeffs_[1] + time_interval_->t2_ * coeffs_[2] + time_interval_->t3_ * coeffs_[3]; } Interval TaylorModel::getTightBound(FCL_REAL t0, FCL_REAL t1) const { if(t0 < time_interval_->t_[0]) t0 = time_interval_->t_[0]; if(t1 > time_interval_->t_[1]) t1 = time_interval_->t_[1]; if(coeffs_[3] == 0) { register FCL_REAL a = -coeffs_[1] / (2 * coeffs_[2]); Interval polybounds; if(a <= t1 && a >= t0) { FCL_REAL AQ = coeffs_[0] + a * (coeffs_[1] + a * coeffs_[2]); register FCL_REAL t = t0; FCL_REAL LQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]); t = t1; FCL_REAL RQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]); FCL_REAL minQ = LQ, maxQ = RQ; if(LQ > RQ) { minQ = RQ; maxQ = LQ; } if(minQ > AQ) minQ = AQ; if(maxQ < AQ) maxQ = AQ; polybounds.setValue(minQ, maxQ); } else { register FCL_REAL t = t0; FCL_REAL LQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]); t = t1; FCL_REAL RQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]); if(LQ > RQ) polybounds.setValue(RQ, LQ); else polybounds.setValue(LQ, RQ); } return polybounds + r_; } else { register FCL_REAL t = t0; FCL_REAL LQ = coeffs_[0] + t * (coeffs_[1] + t * (coeffs_[2] + t * coeffs_[3])); t = t1; FCL_REAL RQ = coeffs_[0] + t * (coeffs_[1] + t * (coeffs_[2] + t * coeffs_[3])); if(LQ > RQ) { FCL_REAL tmp = LQ; LQ = RQ; RQ = tmp; } // derivative: c1+2*c2*t+3*c3*t^2 FCL_REAL delta = coeffs_[2] * coeffs_[2] - 3 * coeffs_[1] * coeffs_[3]; if(delta < 0) return Interval(LQ, RQ) + r_; FCL_REAL r1 = (-coeffs_[2]-sqrt(delta))/(3*coeffs_[3]); FCL_REAL r2 = (-coeffs_[2]+sqrt(delta))/(3*coeffs_[3]); if(r1 <= t1 && r1 >= t0) { FCL_REAL Q = coeffs_[0] + r1 * (coeffs_[1] + r1 * (coeffs_[2] + r1 * coeffs_[3])); if(Q < LQ) LQ = Q; else if(Q > RQ) RQ = Q; } if(r2 <= t1 && r2 >= t0) { FCL_REAL Q = coeffs_[0] + r2 * (coeffs_[1] + r2 * (coeffs_[2] + r2 * coeffs_[3])); if(Q < LQ) LQ = Q; else if(Q > RQ) RQ = Q; } return Interval(LQ, RQ) + r_; } } Interval TaylorModel::getTightBound() const { return getTightBound(time_interval_->t_[0], time_interval_->t_[1]); } void TaylorModel::setZero() { coeffs_[0] = coeffs_[1] = coeffs_[2] = coeffs_[3] = 0; r_.setValue(0); } TaylorModel operator * (FCL_REAL d, const TaylorModel& a) { TaylorModel res(a); res.coeff(0) *= d; res.coeff(1) *= d; res.coeff(2) *= d; res.coeff(3) *= d; res.remainder() *= d; return res; } TaylorModel operator + (FCL_REAL d, const TaylorModel& a) { return a + d; } TaylorModel operator - (FCL_REAL d, const TaylorModel& a) { return -a + d; } void generateTaylorModelForCosFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0) { FCL_REAL a = tm.getTimeInterval()->t_.center(); FCL_REAL t = w * a + q0; FCL_REAL w2 = w * w; FCL_REAL fa = cos(t); FCL_REAL fda = -w*sin(t); FCL_REAL fdda = -w2*fa; FCL_REAL fddda = -w2*fda; tm.coeff(0) = fa-a*(fda-0.5*a*(fdda-1.0/3.0*a*fddda)); tm.coeff(1) = fda-a*fdda+0.5*a*a*fddda; tm.coeff(2) = 0.5*(fdda-a*fddda); tm.coeff(3) = 1.0/6.0*fddda; // compute bounds for w^3 cos(wt+q0)/16, t \in [t0, t1] Interval fddddBounds; if(w == 0) fddddBounds.setValue(0); else { FCL_REAL cosQL = cos(tm.getTimeInterval()->t_[0] * w + q0); FCL_REAL cosQR = cos(tm.getTimeInterval()->t_[1] * w + q0); if(cosQL < cosQR) fddddBounds.setValue(cosQL, cosQR); else fddddBounds.setValue(cosQR, cosQL); // enlarge to handle round-off errors fddddBounds[0] -= 1e-15; fddddBounds[1] += 1e-15; // cos reaches maximum if there exists an integer k in [(w*t0+q0)/2pi, (w*t1+q0)/2pi]; // cos reaches minimum if there exists an integer k in [(w*t0+q0-pi)/2pi, (w*t1+q0-pi)/2pi] FCL_REAL k1 = (tm.getTimeInterval()->t_[0] * w + q0) / (2 * constants::pi); FCL_REAL k2 = (tm.getTimeInterval()->t_[1] * w + q0) / (2 * constants::pi); if(w > 0) { if(ceil(k2) - floor(k1) > 1) fddddBounds[1] = 1; k1 -= 0.5; k2 -= 0.5; if(ceil(k2) - floor(k1) > 1) fddddBounds[0] = -1; } else { if(ceil(k1) - floor(k2) > 1) fddddBounds[1] = 1; k1 -= 0.5; k2 -= 0.5; if(ceil(k1) - floor(k2) > 1) fddddBounds[0] = -1; } } FCL_REAL w4 = w2 * w2; fddddBounds *= w4; FCL_REAL midSize = 0.5 * (tm.getTimeInterval()->t_[1] - tm.getTimeInterval()->t_[0]); FCL_REAL midSize2 = midSize * midSize; FCL_REAL midSize4 = midSize2 * midSize2; // [0, midSize4] * fdddBounds if(fddddBounds[0] > 0) tm.remainder().setValue(0, fddddBounds[1] * midSize4 * (1.0 / 24)); else if(fddddBounds[0] < 0) tm.remainder().setValue(fddddBounds[0] * midSize4 * (1.0 / 24), 0); else tm.remainder().setValue(fddddBounds[0] * midSize4 * (1.0 / 24), fddddBounds[1] * midSize4 * (1.0 / 24)); } void generateTaylorModelForSinFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0) { FCL_REAL a = tm.getTimeInterval()->t_.center(); FCL_REAL t = w * a + q0; FCL_REAL w2 = w * w; FCL_REAL fa = sin(t); FCL_REAL fda = w*cos(t); FCL_REAL fdda = -w2*fa; FCL_REAL fddda = -w2*fda; tm.coeff(0) = fa-a*(fda-0.5*a*(fdda-1.0/3.0*a*fddda)); tm.coeff(1) = fda-a*fdda+0.5*a*a*fddda; tm.coeff(2) = 0.5*(fdda-a*fddda); tm.coeff(3) = 1.0/6.0*fddda; // compute bounds for w^3 sin(wt+q0)/16, t \in [t0, t1] Interval fddddBounds; if(w == 0) fddddBounds.setValue(0); else { FCL_REAL sinQL = sin(w * tm.getTimeInterval()->t_[0] + q0); FCL_REAL sinQR = sin(w * tm.getTimeInterval()->t_[1] + q0); if(sinQL < sinQR) fddddBounds.setValue(sinQL, sinQR); else fddddBounds.setValue(sinQR, sinQL); // enlarge to handle round-off errors fddddBounds[0] -= 1e-15; fddddBounds[1] += 1e-15; // sin reaches maximum if there exists an integer k in [(w*t0+q0-pi/2)/2pi, (w*t1+q0-pi/2)/2pi]; // sin reaches minimum if there exists an integer k in [(w*t0+q0-pi-pi/2)/2pi, (w*t1+q0-pi-pi/2)/2pi] FCL_REAL k1 = (tm.getTimeInterval()->t_[0] * w + q0) / (2 * constants::pi) - 0.25; FCL_REAL k2 = (tm.getTimeInterval()->t_[1] * w + q0) / (2 * constants::pi) - 0.25; if(w > 0) { if(ceil(k2) - floor(k1) > 1) fddddBounds[1] = 1; k1 -= 0.5; k2 -= 0.5; if(ceil(k2) - floor(k1) > 1) fddddBounds[0] = -1; } else { if(ceil(k1) - floor(k2) > 1) fddddBounds[1] = 1; k1 -= 0.5; k2 -= 0.5; if(ceil(k1) - floor(k2) > 1) fddddBounds[0] = -1; } FCL_REAL w4 = w2 * w2; fddddBounds *= w4; FCL_REAL midSize = 0.5 * (tm.getTimeInterval()->t_[1] - tm.getTimeInterval()->t_[0]); FCL_REAL midSize2 = midSize * midSize; FCL_REAL midSize4 = midSize2 * midSize2; // [0, midSize4] * fdddBounds if(fddddBounds[0] > 0) tm.remainder().setValue(0, fddddBounds[1] * midSize4 * (1.0 / 24)); else if(fddddBounds[0] < 0) tm.remainder().setValue(fddddBounds[0] * midSize4 * (1.0 / 24), 0); else tm.remainder().setValue(fddddBounds[0] * midSize4 * (1.0 / 24), fddddBounds[1] * midSize4 * (1.0 / 24)); } } void generateTaylorModelForLinearFunc(TaylorModel& tm, FCL_REAL p, FCL_REAL v) { tm.coeff(0) = p; tm.coeff(1) = v; tm.coeff(2) = 0; tm.coeff(3) = 0; tm.remainder()[0] = 0; tm.remainder()[1] = 0; } } fcl-0.5.0/src/ccd/taylor_vector.cpp000066400000000000000000000160111274356571000172110ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/ccd/taylor_vector.h" namespace fcl { TVector3::TVector3() { } TVector3::TVector3(const std::shared_ptr& time_interval) { setTimeInterval(time_interval); } TVector3::TVector3(TaylorModel v[3]) { i_[0] = v[0]; i_[1] = v[1]; i_[2] = v[2]; } TVector3::TVector3(const TaylorModel& v1, const TaylorModel& v2, const TaylorModel& v3) { i_[0] = v1; i_[1] = v2; i_[2] = v3; } TVector3::TVector3(const Vec3f& v, const std::shared_ptr& time_interval) { i_[0] = TaylorModel(v[0], time_interval); i_[1] = TaylorModel(v[1], time_interval); i_[2] = TaylorModel(v[2], time_interval); } void TVector3::setZero() { i_[0].setZero(); i_[1].setZero(); i_[2].setZero(); } TVector3 TVector3::operator + (const TVector3& other) const { return TVector3(i_[0] + other.i_[0], i_[1] + other.i_[1], i_[2] + other.i_[2]); } TVector3 TVector3::operator - (const TVector3& other) const { return TVector3(i_[0] - other.i_[0], i_[1] - other.i_[1], i_[2] - other.i_[2]); } TVector3 TVector3::operator - () const { return TVector3(-i_[0], -i_[1], -i_[2]); } TVector3& TVector3::operator += (const TVector3& other) { i_[0] += other.i_[0]; i_[1] += other.i_[1]; i_[2] += other.i_[2]; return *this; } TVector3& TVector3::operator -= (const TVector3& other) { i_[0] -= other.i_[0]; i_[1] -= other.i_[1]; i_[2] -= other.i_[2]; return *this; } TVector3 TVector3::operator + (const Vec3f& other) const { return TVector3(i_[0] + other[0], i_[1] + other[1], i_[2] + other[2]); } TVector3& TVector3::operator += (const Vec3f& other) { i_[0] += other[0]; i_[1] += other[1]; i_[2] += other[2]; return *this; } TVector3 TVector3::operator - (const Vec3f& other) const { return TVector3(i_[0] - other[0], i_[1] - other[1], i_[2] - other[2]); } TVector3& TVector3::operator -= (const Vec3f& other) { i_[0] -= other[0]; i_[1] -= other[1]; i_[2] -= other[2]; return *this; } TVector3 TVector3::operator * (const TaylorModel& d) const { return TVector3(i_[0] * d, i_[1] * d, i_[2] * d); } TVector3& TVector3::operator *= (const TaylorModel& d) { i_[0] *= d; i_[1] *= d; i_[2] *= d; return *this; } TVector3 TVector3::operator * (FCL_REAL d) const { return TVector3(i_[0] * d, i_[1] * d, i_[2] * d); } TVector3& TVector3::operator *= (FCL_REAL d) { i_[0] *= d; i_[1] *= d; i_[2] *= d; return *this; } const TaylorModel& TVector3::operator [] (size_t i) const { return i_[i]; } TaylorModel& TVector3::operator [] (size_t i) { return i_[i]; } TaylorModel TVector3::dot(const TVector3& other) const { return i_[0] * other.i_[0] + i_[1] * other.i_[1] + i_[2] * other.i_[2]; } TVector3 TVector3::cross(const TVector3& other) const { return TVector3(i_[1] * other.i_[2] - i_[2] * other.i_[1], i_[2] * other.i_[0] - i_[0] * other.i_[2], i_[0] * other.i_[1] - i_[1] * other.i_[0]); } TaylorModel TVector3::dot(const Vec3f& other) const { return i_[0] * other[0] + i_[1] * other[1] + i_[2] * other[2]; } TVector3 TVector3::cross(const Vec3f& other) const { return TVector3(i_[1] * other[2] - i_[2] * other[1], i_[2] * other[0] - i_[0] * other[2], i_[0] * other[1] - i_[1] * other[0]); } FCL_REAL TVector3::volumn() const { return i_[0].getBound().diameter() * i_[1].getBound().diameter() * i_[2].getBound().diameter(); } IVector3 TVector3::getBound() const { return IVector3(i_[0].getBound(), i_[1].getBound(), i_[2].getBound()); } IVector3 TVector3::getBound(FCL_REAL l, FCL_REAL r) const { return IVector3(i_[0].getBound(l, r), i_[1].getBound(l, r), i_[2].getBound(l, r)); } IVector3 TVector3::getBound(FCL_REAL t) const { return IVector3(i_[0].getBound(t), i_[1].getBound(t), i_[2].getBound(t)); } IVector3 TVector3::getTightBound() const { return IVector3(i_[0].getTightBound(), i_[1].getTightBound(), i_[2].getTightBound()); } IVector3 TVector3::getTightBound(FCL_REAL l, FCL_REAL r) const { return IVector3(i_[0].getTightBound(l, r), i_[1].getTightBound(l, r), i_[2].getTightBound(l, r)); } void TVector3::print() const { i_[0].print(); i_[1].print(); i_[2].print(); } TaylorModel TVector3::squareLength() const { return i_[0] * i_[0] + i_[1] * i_[1] + i_[2] * i_[2]; } void TVector3::setTimeInterval(const std::shared_ptr& time_interval) { i_[0].setTimeInterval(time_interval); i_[1].setTimeInterval(time_interval); i_[2].setTimeInterval(time_interval); } void TVector3::setTimeInterval(FCL_REAL l, FCL_REAL r) { i_[0].setTimeInterval(l, r); i_[1].setTimeInterval(l, r); i_[2].setTimeInterval(l, r); } const std::shared_ptr& TVector3::getTimeInterval() const { return i_[0].getTimeInterval(); } void generateTVector3ForLinearFunc(TVector3& v, const Vec3f& position, const Vec3f& velocity) { generateTaylorModelForLinearFunc(v[0], position[0], velocity[0]); generateTaylorModelForLinearFunc(v[1], position[1], velocity[1]); generateTaylorModelForLinearFunc(v[2], position[2], velocity[2]); } TVector3 operator * (const Vec3f& v, const TaylorModel& a) { TVector3 res(a.getTimeInterval()); res[0] = a * v[0]; res[1] = a * v[1]; res[2] = a * v[2]; return res; } TVector3 operator + (const Vec3f& v1, const TVector3& v2) { return v2 + v1; } TVector3 operator - (const Vec3f& v1, const TVector3& v2) { return -v2 + v1; } } fcl-0.5.0/src/collision.cpp000066400000000000000000000126471274356571000155720ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/collision.h" #include "fcl/collision_func_matrix.h" #include "fcl/narrowphase/narrowphase.h" #include namespace fcl { template CollisionFunctionMatrix& getCollisionFunctionLookTable() { static CollisionFunctionMatrix table; return table; }; template std::size_t collide(const CollisionObject* o1, const CollisionObject* o2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { return collide(o1->collisionGeometry().get(), o1->getTransform(), o2->collisionGeometry().get(), o2->getTransform(), nsolver, request, result); } template std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver_, const CollisionRequest& request, CollisionResult& result) { const NarrowPhaseSolver* nsolver = nsolver_; if(!nsolver_) nsolver = new NarrowPhaseSolver(); const CollisionFunctionMatrix& looktable = getCollisionFunctionLookTable(); std::size_t res; if(request.num_max_contacts == 0) { std::cerr << "Warning: should stop early as num_max_contact is " << request.num_max_contacts << " !" << std::endl; res = 0; } else { OBJECT_TYPE object_type1 = o1->getObjectType(); OBJECT_TYPE object_type2 = o2->getObjectType(); NODE_TYPE node_type1 = o1->getNodeType(); NODE_TYPE node_type2 = o2->getNodeType(); if(object_type1 == OT_GEOM && object_type2 == OT_BVH) { if(!looktable.collision_matrix[node_type2][node_type1]) { std::cerr << "Warning: collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported"<< std::endl; res = 0; } else res = looktable.collision_matrix[node_type2][node_type1](o2, tf2, o1, tf1, nsolver, request, result); } else { if(!looktable.collision_matrix[node_type1][node_type2]) { std::cerr << "Warning: collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported"<< std::endl; res = 0; } else res = looktable.collision_matrix[node_type1][node_type2](o1, tf1, o2, tf2, nsolver, request, result); } } if(!nsolver_) delete nsolver; return res; } std::size_t collide(const CollisionObject* o1, const CollisionObject* o2, const CollisionRequest& request, CollisionResult& result) { switch(request.gjk_solver_type) { case GST_LIBCCD: { GJKSolver_libccd solver; return collide(o1, o2, &solver, request, result); } case GST_INDEP: { GJKSolver_indep solver; return collide(o1, o2, &solver, request, result); } default: return -1; // error } } std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result) { switch(request.gjk_solver_type) { case GST_LIBCCD: { GJKSolver_libccd solver; return collide(o1, tf1, o2, tf2, &solver, request, result); } case GST_INDEP: { GJKSolver_indep solver; return collide(o1, tf1, o2, tf2, &solver, request, result); } default: std::cerr << "Warning! Invalid GJK solver" << std::endl; return -1; // error } } } fcl-0.5.0/src/collision_data.cpp000066400000000000000000000043031274356571000165510ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/collision_data.h" namespace fcl { bool comparePenDepth(const ContactPoint& _cp1, const ContactPoint& _cp2) { return _cp1.penetration_depth < _cp2.penetration_depth; } bool CollisionRequest::isSatisfied(const CollisionResult& result) const { return (!enable_cost) && result.isCollision() && (num_max_contacts <= result.numContacts()); } bool DistanceRequest::isSatisfied(const DistanceResult& result) const { return (result.min_distance <= 0); } } fcl-0.5.0/src/collision_func_matrix.cpp000077500000000000000000001126721274356571000201730ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/collision_func_matrix.h" #include "fcl/traversal/traversal_node_setup.h" #include "fcl/collision_node.h" #include "fcl/narrowphase/narrowphase.h" namespace fcl { #if FCL_HAVE_OCTOMAP template std::size_t ShapeOcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { if(request.isSatisfied(result)) return result.numContacts(); ShapeOcTreeCollisionTraversalNode node; const T_SH* obj1 = static_cast(o1); const OcTree* obj2 = static_cast(o2); OcTreeSolver otsolver(nsolver); initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result); collide(&node); return result.numContacts(); } template std::size_t OcTreeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { if(request.isSatisfied(result)) return result.numContacts(); OcTreeShapeCollisionTraversalNode node; const OcTree* obj1 = static_cast(o1); const T_SH* obj2 = static_cast(o2); OcTreeSolver otsolver(nsolver); initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result); collide(&node); return result.numContacts(); } template std::size_t OcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { if(request.isSatisfied(result)) return result.numContacts(); OcTreeCollisionTraversalNode node; const OcTree* obj1 = static_cast(o1); const OcTree* obj2 = static_cast(o2); OcTreeSolver otsolver(nsolver); initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result); collide(&node); return result.numContacts(); } template std::size_t OcTreeBVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { if(request.isSatisfied(result)) return result.numContacts(); if(request.enable_cost && request.use_approximate_cost) { CollisionRequest no_cost_request(request); // request remove cost to avoid the exact but expensive cost computation between mesh and octree no_cost_request.enable_cost = false; // disable cost computation OcTreeMeshCollisionTraversalNode node; const OcTree* obj1 = static_cast(o1); const BVHModel* obj2 = static_cast*>(o2); OcTreeSolver otsolver(nsolver); initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, no_cost_request, result); collide(&node); Box box; Transform3f box_tf; constructBox(obj2->getBV(0).bv, tf2, box, box_tf); // compute the box for BVH's root node box.cost_density = obj2->cost_density; box.threshold_occupied = obj2->threshold_occupied; box.threshold_free = obj2->threshold_free; CollisionRequest only_cost_request(result.numContacts(), false, request.num_max_cost_sources, true, false); // additional cost request, no contacts OcTreeShapeCollide(o1, tf1, &box, box_tf, nsolver, only_cost_request, result); } else { OcTreeMeshCollisionTraversalNode node; const OcTree* obj1 = static_cast(o1); const BVHModel* obj2 = static_cast*>(o2); OcTreeSolver otsolver(nsolver); initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result); collide(&node); } return result.numContacts(); } template std::size_t BVHOcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { if(request.isSatisfied(result)) return result.numContacts(); if(request.enable_cost && request.use_approximate_cost) { CollisionRequest no_cost_request(request); // request remove cost to avoid the exact but expensive cost computation between mesh and octree no_cost_request.enable_cost = false; // disable cost computation MeshOcTreeCollisionTraversalNode node; const BVHModel* obj1 = static_cast*>(o1); const OcTree* obj2 = static_cast(o2); OcTreeSolver otsolver(nsolver); initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, no_cost_request, result); collide(&node); Box box; Transform3f box_tf; constructBox(obj1->getBV(0).bv, tf1, box, box_tf); box.cost_density = obj1->cost_density; box.threshold_occupied = obj1->threshold_occupied; box.threshold_free = obj1->threshold_free; CollisionRequest only_cost_request(result.numContacts(), false, request.num_max_cost_sources, true, false); ShapeOcTreeCollide(&box, box_tf, o2, tf2, nsolver, only_cost_request, result); } else { MeshOcTreeCollisionTraversalNode node; const BVHModel* obj1 = static_cast*>(o1); const OcTree* obj2 = static_cast(o2); OcTreeSolver otsolver(nsolver); initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result); collide(&node); } return result.numContacts(); } #endif template std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { if(request.isSatisfied(result)) return result.numContacts(); ShapeCollisionTraversalNode node; const T_SH1* obj1 = static_cast(o1); const T_SH2* obj2 = static_cast(o2); if(request.enable_cached_gjk_guess) { nsolver->enableCachedGuess(true); nsolver->setCachedGuess(request.cached_gjk_guess); } else { nsolver->enableCachedGuess(true); } initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result); collide(&node); if(request.enable_cached_gjk_guess) result.cached_gjk_guess = nsolver->getCachedGuess(); return result.numContacts(); } template struct BVHShapeCollider { static std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { if(request.isSatisfied(result)) return result.numContacts(); if(request.enable_cost && request.use_approximate_cost) { CollisionRequest no_cost_request(request); no_cost_request.enable_cost = false; MeshShapeCollisionTraversalNode node; const BVHModel* obj1 = static_cast* >(o1); BVHModel* obj1_tmp = new BVHModel(*obj1); Transform3f tf1_tmp = tf1; const T_SH* obj2 = static_cast(o2); initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, no_cost_request, result); fcl::collide(&node); delete obj1_tmp; Box box; Transform3f box_tf; constructBox(obj1->getBV(0).bv, tf1, box, box_tf); box.cost_density = obj1->cost_density; box.threshold_occupied = obj1->threshold_occupied; box.threshold_free = obj1->threshold_free; CollisionRequest only_cost_request(result.numContacts(), false, request.num_max_cost_sources, true, false); ShapeShapeCollide(&box, box_tf, o2, tf2, nsolver, only_cost_request, result); } else { MeshShapeCollisionTraversalNode node; const BVHModel* obj1 = static_cast* >(o1); BVHModel* obj1_tmp = new BVHModel(*obj1); Transform3f tf1_tmp = tf1; const T_SH* obj2 = static_cast(o2); initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request, result); fcl::collide(&node); delete obj1_tmp; } return result.numContacts(); } }; namespace details { template std::size_t orientedBVHShapeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { if(request.isSatisfied(result)) return result.numContacts(); if(request.enable_cost && request.use_approximate_cost) { CollisionRequest no_cost_request(request); no_cost_request.enable_cost = false; OrientMeshShapeCollisionTraveralNode node; const BVHModel* obj1 = static_cast* >(o1); const T_SH* obj2 = static_cast(o2); initialize(node, *obj1, tf1, *obj2, tf2, nsolver, no_cost_request, result); fcl::collide(&node); Box box; Transform3f box_tf; constructBox(obj1->getBV(0).bv, tf1, box, box_tf); box.cost_density = obj1->cost_density; box.threshold_occupied = obj1->threshold_occupied; box.threshold_free = obj1->threshold_free; CollisionRequest only_cost_request(result.numContacts(), false, request.num_max_cost_sources, true, false); ShapeShapeCollide(&box, box_tf, o2, tf2, nsolver, only_cost_request, result); } else { OrientMeshShapeCollisionTraveralNode node; const BVHModel* obj1 = static_cast* >(o1); const T_SH* obj2 = static_cast(o2); initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result); fcl::collide(&node); } return result.numContacts(); } } template struct BVHShapeCollider { static std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { return details::orientedBVHShapeCollide, OBB, T_SH, NarrowPhaseSolver>(o1, tf1, o2, tf2, nsolver, request, result); } }; template struct BVHShapeCollider { static std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { return details::orientedBVHShapeCollide, RSS, T_SH, NarrowPhaseSolver>(o1, tf1, o2, tf2, nsolver, request, result); } }; template struct BVHShapeCollider { static std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { return details::orientedBVHShapeCollide, kIOS, T_SH, NarrowPhaseSolver>(o1, tf1, o2, tf2, nsolver, request, result); } }; template struct BVHShapeCollider { static std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { return details::orientedBVHShapeCollide, OBBRSS, T_SH, NarrowPhaseSolver>(o1, tf1, o2, tf2, nsolver, request, result); } }; template std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result) { if(request.isSatisfied(result)) return result.numContacts(); MeshCollisionTraversalNode node; const BVHModel* obj1 = static_cast* >(o1); const BVHModel* obj2 = static_cast* >(o2); BVHModel* obj1_tmp = new BVHModel(*obj1); Transform3f tf1_tmp = tf1; BVHModel* obj2_tmp = new BVHModel(*obj2); Transform3f tf2_tmp = tf2; initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, request, result); collide(&node); delete obj1_tmp; delete obj2_tmp; return result.numContacts(); } namespace details { template std::size_t orientedMeshCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result) { if(request.isSatisfied(result)) return result.numContacts(); OrientedMeshCollisionTraversalNode node; const BVHModel* obj1 = static_cast* >(o1); const BVHModel* obj2 = static_cast* >(o2); initialize(node, *obj1, tf1, *obj2, tf2, request, result); collide(&node); return result.numContacts(); } } template<> std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result) { return details::orientedMeshCollide(o1, tf1, o2, tf2, request, result); } template<> std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result) { return details::orientedMeshCollide(o1, tf1, o2, tf2, request, result); } template<> std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result) { return details::orientedMeshCollide(o1, tf1, o2, tf2, request, result); } template std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result) { return BVHCollide(o1, tf1, o2, tf2, request, result); } template CollisionFunctionMatrix::CollisionFunctionMatrix() { for(int i = 0; i < NODE_COUNT; ++i) { for(int j = 0; j < NODE_COUNT; ++j) collision_matrix[i][j] = NULL; } collision_matrix[GEOM_BOX][GEOM_BOX] = &ShapeShapeCollide; collision_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeShapeCollide; collision_matrix[GEOM_BOX][GEOM_ELLIPSOID] = &ShapeShapeCollide; collision_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeShapeCollide; collision_matrix[GEOM_BOX][GEOM_CONE] = &ShapeShapeCollide; collision_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeCollide; collision_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeShapeCollide; collision_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeCollide; collision_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeShapeCollide; collision_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeCollide; collision_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeCollide; collision_matrix[GEOM_SPHERE][GEOM_ELLIPSOID] = &ShapeShapeCollide; collision_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeShapeCollide; collision_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeShapeCollide; collision_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeShapeCollide; collision_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeShapeCollide; collision_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeCollide; collision_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeShapeCollide; collision_matrix[GEOM_ELLIPSOID][GEOM_BOX] = &ShapeShapeCollide; collision_matrix[GEOM_ELLIPSOID][GEOM_SPHERE] = &ShapeShapeCollide; collision_matrix[GEOM_ELLIPSOID][GEOM_ELLIPSOID] = &ShapeShapeCollide; collision_matrix[GEOM_ELLIPSOID][GEOM_CAPSULE] = &ShapeShapeCollide; collision_matrix[GEOM_ELLIPSOID][GEOM_CONE] = &ShapeShapeCollide; collision_matrix[GEOM_ELLIPSOID][GEOM_CYLINDER] = &ShapeShapeCollide; collision_matrix[GEOM_ELLIPSOID][GEOM_CONVEX] = &ShapeShapeCollide; collision_matrix[GEOM_ELLIPSOID][GEOM_PLANE] = &ShapeShapeCollide; collision_matrix[GEOM_ELLIPSOID][GEOM_HALFSPACE] = &ShapeShapeCollide; collision_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeCollide; collision_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeCollide; collision_matrix[GEOM_CAPSULE][GEOM_ELLIPSOID] = &ShapeShapeCollide; collision_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeShapeCollide; collision_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeShapeCollide; collision_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeShapeCollide; collision_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeShapeCollide; collision_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeShapeCollide; collision_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeShapeCollide; collision_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeCollide; collision_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeCollide; collision_matrix[GEOM_CONE][GEOM_ELLIPSOID] = &ShapeShapeCollide; collision_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeShapeCollide; collision_matrix[GEOM_CONE][GEOM_CONE] = &ShapeShapeCollide; collision_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeShapeCollide; collision_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeShapeCollide; collision_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeCollide; collision_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeShapeCollide; collision_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeCollide; collision_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeCollide; collision_matrix[GEOM_CYLINDER][GEOM_ELLIPSOID] = &ShapeShapeCollide; collision_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeShapeCollide; collision_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeShapeCollide; collision_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeShapeCollide; collision_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeShapeCollide; collision_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeShapeCollide; collision_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX][GEOM_ELLIPSOID] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeShapeCollide; collision_matrix[GEOM_CONVEX][GEOM_HALFSPACE] = &ShapeShapeCollide; collision_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeCollide; collision_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeCollide; collision_matrix[GEOM_PLANE][GEOM_ELLIPSOID] = &ShapeShapeCollide; collision_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeShapeCollide; collision_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeShapeCollide; collision_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeShapeCollide; collision_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeShapeCollide; collision_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeCollide; collision_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeShapeCollide; collision_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeShapeCollide; collision_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeShapeCollide; collision_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = &ShapeShapeCollide; collision_matrix[GEOM_HALFSPACE][GEOM_CONE] = &ShapeShapeCollide; collision_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = &ShapeShapeCollide; collision_matrix[GEOM_HALFSPACE][GEOM_CONVEX] = &ShapeShapeCollide; collision_matrix[GEOM_HALFSPACE][GEOM_PLANE] = &ShapeShapeCollide; collision_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeShapeCollide; collision_matrix[BV_AABB][GEOM_BOX] = &BVHShapeCollider::collide; collision_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeCollider::collide; collision_matrix[BV_AABB][GEOM_ELLIPSOID] = &BVHShapeCollider::collide; collision_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeCollider::collide; collision_matrix[BV_AABB][GEOM_CONE] = &BVHShapeCollider::collide; collision_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeCollider::collide; collision_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeCollider::collide; collision_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeCollider::collide; collision_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeCollider::collide; collision_matrix[BV_OBB][GEOM_BOX] = &BVHShapeCollider::collide; collision_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeCollider::collide; collision_matrix[BV_OBB][GEOM_ELLIPSOID] = &BVHShapeCollider::collide; collision_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeCollider::collide; collision_matrix[BV_OBB][GEOM_CONE] = &BVHShapeCollider::collide; collision_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeCollider::collide; collision_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeCollider::collide; collision_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeCollider::collide; collision_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeCollider::collide; collision_matrix[BV_RSS][GEOM_BOX] = &BVHShapeCollider::collide; collision_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeCollider::collide; collision_matrix[BV_RSS][GEOM_ELLIPSOID] = &BVHShapeCollider::collide; collision_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeCollider::collide; collision_matrix[BV_RSS][GEOM_CONE] = &BVHShapeCollider::collide; collision_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeCollider::collide; collision_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeCollider::collide; collision_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeCollider::collide; collision_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeCollider::collide; collision_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeCollider, Box, NarrowPhaseSolver>::collide; collision_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeCollider, Sphere, NarrowPhaseSolver>::collide; collision_matrix[BV_KDOP16][GEOM_ELLIPSOID] = &BVHShapeCollider, Ellipsoid, NarrowPhaseSolver>::collide; collision_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeCollider, Capsule, NarrowPhaseSolver>::collide; collision_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeCollider, Cone, NarrowPhaseSolver>::collide; collision_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeCollider, Cylinder, NarrowPhaseSolver>::collide; collision_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeCollider, Convex, NarrowPhaseSolver>::collide; collision_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeCollider, Plane, NarrowPhaseSolver>::collide; collision_matrix[BV_KDOP16][GEOM_HALFSPACE] = &BVHShapeCollider, Halfspace, NarrowPhaseSolver>::collide; collision_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeCollider, Box, NarrowPhaseSolver>::collide; collision_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeCollider, Sphere, NarrowPhaseSolver>::collide; collision_matrix[BV_KDOP18][GEOM_ELLIPSOID] = &BVHShapeCollider, Ellipsoid, NarrowPhaseSolver>::collide; collision_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeCollider, Capsule, NarrowPhaseSolver>::collide; collision_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeCollider, Cone, NarrowPhaseSolver>::collide; collision_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeCollider, Cylinder, NarrowPhaseSolver>::collide; collision_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeCollider, Convex, NarrowPhaseSolver>::collide; collision_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeCollider, Plane, NarrowPhaseSolver>::collide; collision_matrix[BV_KDOP18][GEOM_HALFSPACE] = &BVHShapeCollider, Halfspace, NarrowPhaseSolver>::collide; collision_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeCollider, Box, NarrowPhaseSolver>::collide; collision_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeCollider, Sphere, NarrowPhaseSolver>::collide; collision_matrix[BV_KDOP24][GEOM_ELLIPSOID] = &BVHShapeCollider, Ellipsoid, NarrowPhaseSolver>::collide; collision_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeCollider, Capsule, NarrowPhaseSolver>::collide; collision_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeCollider, Cone, NarrowPhaseSolver>::collide; collision_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeCollider, Cylinder, NarrowPhaseSolver>::collide; collision_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeCollider, Convex, NarrowPhaseSolver>::collide; collision_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeCollider, Plane, NarrowPhaseSolver>::collide; collision_matrix[BV_KDOP24][GEOM_HALFSPACE] = &BVHShapeCollider, Halfspace, NarrowPhaseSolver>::collide; collision_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeCollider::collide; collision_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeCollider::collide; collision_matrix[BV_kIOS][GEOM_ELLIPSOID] = &BVHShapeCollider::collide; collision_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeCollider::collide; collision_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeCollider::collide; collision_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeCollider::collide; collision_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeCollider::collide; collision_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeCollider::collide; collision_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeCollider::collide; collision_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeCollider::collide; collision_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeCollider::collide; collision_matrix[BV_OBBRSS][GEOM_ELLIPSOID] = &BVHShapeCollider::collide; collision_matrix[BV_OBBRSS][GEOM_CAPSULE] = &BVHShapeCollider::collide; collision_matrix[BV_OBBRSS][GEOM_CONE] = &BVHShapeCollider::collide; collision_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeCollider::collide; collision_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeCollider::collide; collision_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeCollider::collide; collision_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeCollider::collide; collision_matrix[BV_AABB][BV_AABB] = &BVHCollide; collision_matrix[BV_OBB][BV_OBB] = &BVHCollide; collision_matrix[BV_RSS][BV_RSS] = &BVHCollide; collision_matrix[BV_KDOP16][BV_KDOP16] = &BVHCollide, NarrowPhaseSolver>; collision_matrix[BV_KDOP18][BV_KDOP18] = &BVHCollide, NarrowPhaseSolver>; collision_matrix[BV_KDOP24][BV_KDOP24] = &BVHCollide, NarrowPhaseSolver>; collision_matrix[BV_kIOS][BV_kIOS] = &BVHCollide; collision_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHCollide; #if FCL_HAVE_OCTOMAP collision_matrix[GEOM_OCTREE][GEOM_BOX] = &OcTreeShapeCollide; collision_matrix[GEOM_OCTREE][GEOM_SPHERE] = &OcTreeShapeCollide; collision_matrix[GEOM_OCTREE][GEOM_ELLIPSOID] = &OcTreeShapeCollide; collision_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &OcTreeShapeCollide; collision_matrix[GEOM_OCTREE][GEOM_CONE] = &OcTreeShapeCollide; collision_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &OcTreeShapeCollide; collision_matrix[GEOM_OCTREE][GEOM_CONVEX] = &OcTreeShapeCollide; collision_matrix[GEOM_OCTREE][GEOM_PLANE] = &OcTreeShapeCollide; collision_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &OcTreeShapeCollide; collision_matrix[GEOM_BOX][GEOM_OCTREE] = &ShapeOcTreeCollide; collision_matrix[GEOM_SPHERE][GEOM_OCTREE] = &ShapeOcTreeCollide; collision_matrix[GEOM_ELLIPSOID][GEOM_OCTREE] = &ShapeOcTreeCollide; collision_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &ShapeOcTreeCollide; collision_matrix[GEOM_CONE][GEOM_OCTREE] = &ShapeOcTreeCollide; collision_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &ShapeOcTreeCollide; collision_matrix[GEOM_CONVEX][GEOM_OCTREE] = &ShapeOcTreeCollide; collision_matrix[GEOM_PLANE][GEOM_OCTREE] = &ShapeOcTreeCollide; collision_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &ShapeOcTreeCollide; collision_matrix[GEOM_OCTREE][GEOM_OCTREE] = &OcTreeCollide; collision_matrix[GEOM_OCTREE][BV_AABB] = &OcTreeBVHCollide; collision_matrix[GEOM_OCTREE][BV_OBB] = &OcTreeBVHCollide; collision_matrix[GEOM_OCTREE][BV_RSS] = &OcTreeBVHCollide; collision_matrix[GEOM_OCTREE][BV_OBBRSS] = &OcTreeBVHCollide; collision_matrix[GEOM_OCTREE][BV_kIOS] = &OcTreeBVHCollide; collision_matrix[GEOM_OCTREE][BV_KDOP16] = &OcTreeBVHCollide, NarrowPhaseSolver>; collision_matrix[GEOM_OCTREE][BV_KDOP18] = &OcTreeBVHCollide, NarrowPhaseSolver>; collision_matrix[GEOM_OCTREE][BV_KDOP24] = &OcTreeBVHCollide, NarrowPhaseSolver>; collision_matrix[BV_AABB][GEOM_OCTREE] = &BVHOcTreeCollide; collision_matrix[BV_OBB][GEOM_OCTREE] = &BVHOcTreeCollide; collision_matrix[BV_RSS][GEOM_OCTREE] = &BVHOcTreeCollide; collision_matrix[BV_OBBRSS][GEOM_OCTREE] = &BVHOcTreeCollide; collision_matrix[BV_kIOS][GEOM_OCTREE] = &BVHOcTreeCollide; collision_matrix[BV_KDOP16][GEOM_OCTREE] = &BVHOcTreeCollide, NarrowPhaseSolver>; collision_matrix[BV_KDOP18][GEOM_OCTREE] = &BVHOcTreeCollide, NarrowPhaseSolver>; collision_matrix[BV_KDOP24][GEOM_OCTREE] = &BVHOcTreeCollide, NarrowPhaseSolver>; #endif } template struct CollisionFunctionMatrix; template struct CollisionFunctionMatrix; } fcl-0.5.0/src/collision_node.cpp000066400000000000000000000067271274356571000166010ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/collision_node.h" #include "fcl/traversal/traversal_recurse.h" namespace fcl { void collide(CollisionTraversalNodeBase* node, BVHFrontList* front_list) { if(front_list && front_list->size() > 0) { propagateBVHFrontListCollisionRecurse(node, front_list); } else { collisionRecurse(node, 0, 0, front_list); } } void collide2(MeshCollisionTraversalNodeOBB* node, BVHFrontList* front_list) { if(front_list && front_list->size() > 0) { propagateBVHFrontListCollisionRecurse(node, front_list); } else { Matrix3f Rtemp, R; Vec3f Ttemp, T; Rtemp = node->R * node->model2->getBV(0).getOrientation(); R = node->model1->getBV(0).getOrientation().transposeTimes(Rtemp); Ttemp = node->R * node->model2->getBV(0).getCenter() + node->T; Ttemp -= node->model1->getBV(0).getCenter(); T = node->model1->getBV(0).getOrientation().transposeTimes(Ttemp); collisionRecurse(node, 0, 0, R, T, front_list); } } void collide2(MeshCollisionTraversalNodeRSS* node, BVHFrontList* front_list) { if(front_list && front_list->size() > 0) { propagateBVHFrontListCollisionRecurse(node, front_list); } else { collisionRecurse(node, 0, 0, node->R, node->T, front_list); } } void selfCollide(CollisionTraversalNodeBase* node, BVHFrontList* front_list) { if(front_list && front_list->size() > 0) { propagateBVHFrontListCollisionRecurse(node, front_list); } else { selfCollisionRecurse(node, 0, front_list); } } void distance(DistanceTraversalNodeBase* node, BVHFrontList* front_list, int qsize) { node->preprocess(); if(qsize <= 2) distanceRecurse(node, 0, 0, front_list); else distanceQueueRecurse(node, 0, 0, front_list, qsize); node->postprocess(); } } fcl-0.5.0/src/continuous_collision.cpp000066400000000000000000000266231274356571000200570ustar00rootroot00000000000000#include "fcl/collision.h" #include "fcl/continuous_collision.h" #include "fcl/ccd/motion.h" #include "fcl/BVH/BVH_model.h" #include "fcl/traversal/traversal_node_bvhs.h" #include "fcl/traversal/traversal_node_setup.h" #include "fcl/collision_node.h" #include "fcl/ccd/conservative_advancement.h" #include namespace fcl { template ConservativeAdvancementFunctionMatrix& getConservativeAdvancementFunctionLookTable() { static ConservativeAdvancementFunctionMatrix table; return table; } MotionBasePtr getMotionBase(const Transform3f& tf_beg, const Transform3f& tf_end, CCDMotionType motion_type) { switch(motion_type) { case CCDM_TRANS: return MotionBasePtr(new TranslationMotion(tf_beg, tf_end)); break; case CCDM_LINEAR: return MotionBasePtr(new InterpMotion(tf_beg, tf_end)); break; case CCDM_SCREW: return MotionBasePtr(new ScrewMotion(tf_beg, tf_end)); break; case CCDM_SPLINE: return MotionBasePtr(new SplineMotion(tf_beg, tf_end)); break; default: return MotionBasePtr(); } } FCL_REAL continuousCollideNaive(const CollisionGeometry* o1, const MotionBase* motion1, const CollisionGeometry* o2, const MotionBase* motion2, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result) { std::size_t n_iter = std::min(request.num_max_iterations, (std::size_t)ceil(1 / request.toc_err)); Transform3f cur_tf1, cur_tf2; for(std::size_t i = 0; i < n_iter; ++i) { FCL_REAL t = i / (FCL_REAL) (n_iter - 1); motion1->integrate(t); motion2->integrate(t); motion1->getCurrentTransform(cur_tf1); motion2->getCurrentTransform(cur_tf2); CollisionRequest c_request; CollisionResult c_result; if(collide(o1, cur_tf1, o2, cur_tf2, c_request, c_result)) { result.is_collide = true; result.time_of_contact = t; result.contact_tf1 = cur_tf1; result.contact_tf2 = cur_tf2; return t; } } result.is_collide = false; result.time_of_contact = FCL_REAL(1); return result.time_of_contact; } namespace details { template FCL_REAL continuousCollideBVHPolynomial(const CollisionGeometry* o1_, const TranslationMotion* motion1, const CollisionGeometry* o2_, const TranslationMotion* motion2, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result) { const BVHModel* o1__ = static_cast*>(o1_); const BVHModel* o2__ = static_cast*>(o2_); // ugly, but lets do it now. BVHModel* o1 = const_cast*>(o1__); BVHModel* o2 = const_cast*>(o2__); std::vector new_v1(o1->num_vertices); std::vector new_v2(o2->num_vertices); for(std::size_t i = 0; i < new_v1.size(); ++i) new_v1[i] = o1->vertices[i] + motion1->getVelocity(); for(std::size_t i = 0; i < new_v2.size(); ++i) new_v2[i] = o2->vertices[i] + motion2->getVelocity(); o1->beginUpdateModel(); o1->updateSubModel(new_v1); o1->endUpdateModel(true, true); o2->beginUpdateModel(); o2->updateSubModel(new_v2); o2->endUpdateModel(true, true); MeshContinuousCollisionTraversalNode node; CollisionRequest c_request; motion1->integrate(0); motion2->integrate(0); Transform3f tf1, tf2; motion1->getCurrentTransform(tf1); motion2->getCurrentTransform(tf2); if(!initialize(node, *o1, tf1, *o2, tf2, c_request)) return -1.0; collide(&node); result.is_collide = (node.pairs.size() > 0); result.time_of_contact = node.time_of_contact; if(result.is_collide) { motion1->integrate(node.time_of_contact); motion2->integrate(node.time_of_contact); motion1->getCurrentTransform(tf1); motion2->getCurrentTransform(tf2); result.contact_tf1 = tf1; result.contact_tf2 = tf2; } return result.time_of_contact; } } FCL_REAL continuousCollideBVHPolynomial(const CollisionGeometry* o1, const TranslationMotion* motion1, const CollisionGeometry* o2, const TranslationMotion* motion2, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result) { switch(o1->getNodeType()) { case BV_AABB: if(o2->getNodeType() == BV_AABB) return details::continuousCollideBVHPolynomial(o1, motion1, o2, motion2, request, result); break; case BV_OBB: if(o2->getNodeType() == BV_OBB) return details::continuousCollideBVHPolynomial(o1, motion1, o2, motion2, request, result); break; case BV_RSS: if(o2->getNodeType() == BV_RSS) return details::continuousCollideBVHPolynomial(o1, motion1, o2, motion2, request, result); break; case BV_kIOS: if(o2->getNodeType() == BV_kIOS) return details::continuousCollideBVHPolynomial(o1, motion1, o2, motion2, request, result); break; case BV_OBBRSS: if(o2->getNodeType() == BV_OBBRSS) return details::continuousCollideBVHPolynomial(o1, motion1, o2, motion2, request, result); break; case BV_KDOP16: if(o2->getNodeType() == BV_KDOP16) return details::continuousCollideBVHPolynomial >(o1, motion1, o2, motion2, request, result); break; case BV_KDOP18: if(o2->getNodeType() == BV_KDOP18) return details::continuousCollideBVHPolynomial >(o1, motion1, o2, motion2, request, result); break; case BV_KDOP24: if(o2->getNodeType() == BV_KDOP24) return details::continuousCollideBVHPolynomial >(o1, motion1, o2, motion2, request, result); break; default: ; } std::cerr << "Warning: BV type not supported by polynomial solver CCD" << std::endl; return -1; } namespace details { template FCL_REAL continuousCollideConservativeAdvancement(const CollisionGeometry* o1, const MotionBase* motion1, const CollisionGeometry* o2, const MotionBase* motion2, const NarrowPhaseSolver* nsolver_, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result) { const NarrowPhaseSolver* nsolver = nsolver_; if(!nsolver_) nsolver = new NarrowPhaseSolver(); const ConservativeAdvancementFunctionMatrix& looktable = getConservativeAdvancementFunctionLookTable(); NODE_TYPE node_type1 = o1->getNodeType(); NODE_TYPE node_type2 = o2->getNodeType(); FCL_REAL res = -1; if(!looktable.conservative_advancement_matrix[node_type1][node_type2]) { std::cerr << "Warning: collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported"<< std::endl; } else { res = looktable.conservative_advancement_matrix[node_type1][node_type2](o1, motion1, o2, motion2, nsolver, request, result); } if(!nsolver_) delete nsolver; if(result.is_collide) { motion1->integrate(result.time_of_contact); motion2->integrate(result.time_of_contact); Transform3f tf1, tf2; motion1->getCurrentTransform(tf1); motion2->getCurrentTransform(tf2); result.contact_tf1 = tf1; result.contact_tf2 = tf2; } return res; } } FCL_REAL continuousCollideConservativeAdvancement(const CollisionGeometry* o1, const MotionBase* motion1, const CollisionGeometry* o2, const MotionBase* motion2, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result) { switch(request.gjk_solver_type) { case GST_LIBCCD: { GJKSolver_libccd solver; return details::continuousCollideConservativeAdvancement(o1, motion1, o2, motion2, &solver, request, result); } case GST_INDEP: { GJKSolver_indep solver; return details::continuousCollideConservativeAdvancement(o1, motion1, o2, motion2, &solver, request, result); } default: return -1; } } FCL_REAL continuousCollide(const CollisionGeometry* o1, const MotionBase* motion1, const CollisionGeometry* o2, const MotionBase* motion2, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result) { switch(request.ccd_solver_type) { case CCDC_NAIVE: return continuousCollideNaive(o1, motion1, o2, motion2, request, result); break; case CCDC_CONSERVATIVE_ADVANCEMENT: return continuousCollideConservativeAdvancement(o1, motion1, o2, motion2, request, result); break; case CCDC_RAY_SHOOTING: if(o1->getObjectType() == OT_GEOM && o2->getObjectType() == OT_GEOM && request.ccd_motion_type == CCDM_TRANS) { } else std::cerr << "Warning! Invalid continuous collision setting" << std::endl; break; case CCDC_POLYNOMIAL_SOLVER: if(o1->getObjectType() == OT_BVH && o2->getObjectType() == OT_BVH && request.ccd_motion_type == CCDM_TRANS) { return continuousCollideBVHPolynomial(o1, (const TranslationMotion*)motion1, o2, (const TranslationMotion*)motion2, request, result); } else std::cerr << "Warning! Invalid continuous collision checking" << std::endl; break; default: std::cerr << "Warning! Invalid continuous collision setting" << std::endl; } return -1; } FCL_REAL continuousCollide(const CollisionGeometry* o1, const Transform3f& tf1_beg, const Transform3f& tf1_end, const CollisionGeometry* o2, const Transform3f& tf2_beg, const Transform3f& tf2_end, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result) { MotionBasePtr motion1 = getMotionBase(tf1_beg, tf1_end, request.ccd_motion_type); MotionBasePtr motion2 = getMotionBase(tf2_beg, tf2_end, request.ccd_motion_type); return continuousCollide(o1, motion1.get(), o2, motion2.get(), request, result); } FCL_REAL continuousCollide(const CollisionObject* o1, const Transform3f& tf1_end, const CollisionObject* o2, const Transform3f& tf2_end, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result) { return continuousCollide(o1->collisionGeometry().get(), o1->getTransform(), tf1_end, o2->collisionGeometry().get(), o2->getTransform(), tf2_end, request, result); } FCL_REAL collide(const ContinuousCollisionObject* o1, const ContinuousCollisionObject* o2, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result) { return continuousCollide(o1->collisionGeometry().get(), o1->getMotion(), o2->collisionGeometry().get(), o2->getMotion(), request, result); } } fcl-0.5.0/src/distance.cpp000066400000000000000000000121021274356571000153530ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/distance.h" #include "fcl/distance_func_matrix.h" #include "fcl/narrowphase/narrowphase.h" #include namespace fcl { template DistanceFunctionMatrix& getDistanceFunctionLookTable() { static DistanceFunctionMatrix table; return table; } template FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const NarrowPhaseSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { return distance(o1->collisionGeometry().get(), o1->getTransform(), o2->collisionGeometry().get(), o2->getTransform(), nsolver, request, result); } template FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver_, const DistanceRequest& request, DistanceResult& result) { const NarrowPhaseSolver* nsolver = nsolver_; if(!nsolver_) nsolver = new NarrowPhaseSolver(); const DistanceFunctionMatrix& looktable = getDistanceFunctionLookTable(); OBJECT_TYPE object_type1 = o1->getObjectType(); NODE_TYPE node_type1 = o1->getNodeType(); OBJECT_TYPE object_type2 = o2->getObjectType(); NODE_TYPE node_type2 = o2->getNodeType(); FCL_REAL res = std::numeric_limits::max(); if(object_type1 == OT_GEOM && object_type2 == OT_BVH) { if(!looktable.distance_matrix[node_type2][node_type1]) { std::cerr << "Warning: distance function between node type " << node_type1 << " and node type " << node_type2 << " is not supported" << std::endl; } else { res = looktable.distance_matrix[node_type2][node_type1](o2, tf2, o1, tf1, nsolver, request, result); } } else { if(!looktable.distance_matrix[node_type1][node_type2]) { std::cerr << "Warning: distance function between node type " << node_type1 << " and node type " << node_type2 << " is not supported" << std::endl; } else { res = looktable.distance_matrix[node_type1][node_type2](o1, tf1, o2, tf2, nsolver, request, result); } } if(!nsolver_) delete nsolver; return res; } FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const DistanceRequest& request, DistanceResult& result) { switch(request.gjk_solver_type) { case GST_LIBCCD: { GJKSolver_libccd solver; return distance(o1, o2, &solver, request, result); } case GST_INDEP: { GJKSolver_indep solver; return distance(o1, o2, &solver, request, result); } default: return -1; // error } } FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const DistanceRequest& request, DistanceResult& result) { switch(request.gjk_solver_type) { case GST_LIBCCD: { GJKSolver_libccd solver; return distance(o1, tf1, o2, tf2, &solver, request, result); } case GST_INDEP: { GJKSolver_indep solver; return distance(o1, tf1, o2, tf2, &solver, request, result); } default: return -1; } } } fcl-0.5.0/src/distance_func_matrix.cpp000077500000000000000000000775041274356571000177760ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/distance_func_matrix.h" #include "fcl/collision_node.h" #include "fcl/traversal/traversal_node_setup.h" #include "fcl/narrowphase/narrowphase.h" namespace fcl { #if FCL_HAVE_OCTOMAP template FCL_REAL ShapeOcTreeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { if(request.isSatisfied(result)) return result.min_distance; ShapeOcTreeDistanceTraversalNode node; const T_SH* obj1 = static_cast(o1); const OcTree* obj2 = static_cast(o2); OcTreeSolver otsolver(nsolver); initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result); distance(&node); return result.min_distance; } template FCL_REAL OcTreeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { if(request.isSatisfied(result)) return result.min_distance; OcTreeShapeDistanceTraversalNode node; const OcTree* obj1 = static_cast(o1); const T_SH* obj2 = static_cast(o2); OcTreeSolver otsolver(nsolver); initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result); distance(&node); return result.min_distance; } template FCL_REAL OcTreeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { if(request.isSatisfied(result)) return result.min_distance; OcTreeDistanceTraversalNode node; const OcTree* obj1 = static_cast(o1); const OcTree* obj2 = static_cast(o2); OcTreeSolver otsolver(nsolver); initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result); distance(&node); return result.min_distance; } template FCL_REAL BVHOcTreeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { if(request.isSatisfied(result)) return result.min_distance; MeshOcTreeDistanceTraversalNode node; const BVHModel* obj1 = static_cast*>(o1); const OcTree* obj2 = static_cast(o2); OcTreeSolver otsolver(nsolver); initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result); distance(&node); return result.min_distance; } template FCL_REAL OcTreeBVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { if(request.isSatisfied(result)) return result.min_distance; OcTreeMeshDistanceTraversalNode node; const OcTree* obj1 = static_cast(o1); const BVHModel* obj2 = static_cast*>(o2); OcTreeSolver otsolver(nsolver); initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result); distance(&node); return result.min_distance; } #endif template FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { if(request.isSatisfied(result)) return result.min_distance; ShapeDistanceTraversalNode node; const T_SH1* obj1 = static_cast(o1); const T_SH2* obj2 = static_cast(o2); initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result); distance(&node); return result.min_distance; } template struct BVHShapeDistancer { static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { if(request.isSatisfied(result)) return result.min_distance; MeshShapeDistanceTraversalNode node; const BVHModel* obj1 = static_cast* >(o1); BVHModel* obj1_tmp = new BVHModel(*obj1); Transform3f tf1_tmp = tf1; const T_SH* obj2 = static_cast(o2); initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request, result); fcl::distance(&node); delete obj1_tmp; return result.min_distance; } }; namespace details { template FCL_REAL orientedBVHShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { if(request.isSatisfied(result)) return result.min_distance; OrientedMeshShapeDistanceTraversalNode node; const BVHModel* obj1 = static_cast* >(o1); const T_SH* obj2 = static_cast(o2); initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result); fcl::distance(&node); return result.min_distance; } } template struct BVHShapeDistancer { static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { return details::orientedBVHShapeDistance, RSS, T_SH, NarrowPhaseSolver>(o1, tf1, o2, tf2, nsolver, request, result); } }; template struct BVHShapeDistancer { static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { return details::orientedBVHShapeDistance, kIOS, T_SH, NarrowPhaseSolver>(o1, tf1, o2, tf2, nsolver, request, result); } }; template struct BVHShapeDistancer { static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { return details::orientedBVHShapeDistance, OBBRSS, T_SH, NarrowPhaseSolver>(o1, tf1, o2, tf2, nsolver, request, result); } }; template FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const DistanceRequest& request, DistanceResult& result) { if(request.isSatisfied(result)) return result.min_distance; MeshDistanceTraversalNode node; const BVHModel* obj1 = static_cast* >(o1); const BVHModel* obj2 = static_cast* >(o2); BVHModel* obj1_tmp = new BVHModel(*obj1); Transform3f tf1_tmp = tf1; BVHModel* obj2_tmp = new BVHModel(*obj2); Transform3f tf2_tmp = tf2; initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, request, result); distance(&node); delete obj1_tmp; delete obj2_tmp; return result.min_distance; } namespace details { template FCL_REAL orientedMeshDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const DistanceRequest& request, DistanceResult& result) { if(request.isSatisfied(result)) return result.min_distance; OrientedMeshDistanceTraversalNode node; const BVHModel* obj1 = static_cast* >(o1); const BVHModel* obj2 = static_cast* >(o2); initialize(node, *obj1, tf1, *obj2, tf2, request, result); distance(&node); return result.min_distance; } } template<> FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const DistanceRequest& request, DistanceResult& result) { return details::orientedMeshDistance(o1, tf1, o2, tf2, request, result); } template<> FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const DistanceRequest& request, DistanceResult& result) { return details::orientedMeshDistance(o1, tf1, o2, tf2, request, result); } template<> FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const DistanceRequest& request, DistanceResult& result) { return details::orientedMeshDistance(o1, tf1, o2, tf2, request, result); } template FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const DistanceRequest& request, DistanceResult& result) { return BVHDistance(o1, tf1, o2, tf2, request, result); } template DistanceFunctionMatrix::DistanceFunctionMatrix() { for(int i = 0; i < NODE_COUNT; ++i) { for(int j = 0; j < NODE_COUNT; ++j) distance_matrix[i][j] = NULL; } distance_matrix[GEOM_BOX][GEOM_BOX] = &ShapeShapeDistance; distance_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeShapeDistance; distance_matrix[GEOM_BOX][GEOM_ELLIPSOID] = &ShapeShapeDistance; distance_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeShapeDistance; distance_matrix[GEOM_BOX][GEOM_CONE] = &ShapeShapeDistance; distance_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeDistance; distance_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeShapeDistance; distance_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeDistance; distance_matrix[GEOM_BOX][GEOM_HALFSPACE] = &ShapeShapeDistance; distance_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeDistance; distance_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeDistance; distance_matrix[GEOM_SPHERE][GEOM_ELLIPSOID] = &ShapeShapeDistance; distance_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeShapeDistance; distance_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeShapeDistance; distance_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeShapeDistance; distance_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeShapeDistance; distance_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeDistance; distance_matrix[GEOM_SPHERE][GEOM_HALFSPACE] = &ShapeShapeDistance; distance_matrix[GEOM_ELLIPSOID][GEOM_BOX] = &ShapeShapeDistance; distance_matrix[GEOM_ELLIPSOID][GEOM_SPHERE] = &ShapeShapeDistance; distance_matrix[GEOM_ELLIPSOID][GEOM_ELLIPSOID] = &ShapeShapeDistance; distance_matrix[GEOM_ELLIPSOID][GEOM_CAPSULE] = &ShapeShapeDistance; distance_matrix[GEOM_ELLIPSOID][GEOM_CONE] = &ShapeShapeDistance; distance_matrix[GEOM_ELLIPSOID][GEOM_CYLINDER] = &ShapeShapeDistance; distance_matrix[GEOM_ELLIPSOID][GEOM_CONVEX] = &ShapeShapeDistance; distance_matrix[GEOM_ELLIPSOID][GEOM_PLANE] = &ShapeShapeDistance; distance_matrix[GEOM_ELLIPSOID][GEOM_HALFSPACE] = &ShapeShapeDistance; distance_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeDistance; distance_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeDistance; distance_matrix[GEOM_CAPSULE][GEOM_ELLIPSOID] = &ShapeShapeDistance; distance_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeShapeDistance; distance_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeShapeDistance; distance_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeShapeDistance; distance_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeShapeDistance; distance_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeShapeDistance; distance_matrix[GEOM_CAPSULE][GEOM_HALFSPACE] = &ShapeShapeDistance; distance_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeDistance; distance_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeDistance; distance_matrix[GEOM_CONE][GEOM_ELLIPSOID] = &ShapeShapeDistance; distance_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeShapeDistance; distance_matrix[GEOM_CONE][GEOM_CONE] = &ShapeShapeDistance; distance_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeShapeDistance; distance_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeShapeDistance; distance_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeDistance; distance_matrix[GEOM_CONE][GEOM_HALFSPACE] = &ShapeShapeDistance; distance_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeDistance; distance_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeDistance; distance_matrix[GEOM_CYLINDER][GEOM_ELLIPSOID] = &ShapeShapeDistance; distance_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeShapeDistance; distance_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeShapeDistance; distance_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeShapeDistance; distance_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeShapeDistance; distance_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeShapeDistance; distance_matrix[GEOM_CYLINDER][GEOM_HALFSPACE] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX][GEOM_ELLIPSOID] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeShapeDistance; distance_matrix[GEOM_CONVEX][GEOM_HALFSPACE] = &ShapeShapeDistance; distance_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeDistance; distance_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeDistance; distance_matrix[GEOM_PLANE][GEOM_ELLIPSOID] = &ShapeShapeDistance; distance_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeShapeDistance; distance_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeShapeDistance; distance_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeShapeDistance; distance_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeShapeDistance; distance_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeDistance; distance_matrix[GEOM_PLANE][GEOM_HALFSPACE] = &ShapeShapeDistance; distance_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeShapeDistance; distance_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeShapeDistance; distance_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = &ShapeShapeDistance; distance_matrix[GEOM_HALFSPACE][GEOM_CONE] = &ShapeShapeDistance; distance_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = &ShapeShapeDistance; distance_matrix[GEOM_HALFSPACE][GEOM_CONVEX] = &ShapeShapeDistance; distance_matrix[GEOM_HALFSPACE][GEOM_PLANE] = &ShapeShapeDistance; distance_matrix[GEOM_HALFSPACE][GEOM_HALFSPACE] = &ShapeShapeDistance; /* AABB distance not implemented */ /* distance_matrix[BV_AABB][GEOM_BOX] = &BVHShapeDistancer::distance; distance_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeDistancer::distance; distance_matrix[BV_AABB][GEOM_ELLIPSOID] = &BVHShapeDistancer::distance; distance_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeDistancer::distance; distance_matrix[BV_AABB][GEOM_CONE] = &BVHShapeDistancer::distance; distance_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeDistancer::distance; distance_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeDistancer::distance; distance_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeDistancer::distance; distance_matrix[BV_AABB][GEOM_HALFSPACE] = &BVHShapeDistancer::distance; distance_matrix[BV_OBB][GEOM_BOX] = &BVHShapeDistancer::distance; distance_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeDistancer::distance; distance_matrix[BV_OBB][GEOM_ELLIPSOID] = &BVHShapeDistancer::distance; distance_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeDistancer::distance; distance_matrix[BV_OBB][GEOM_CONE] = &BVHShapeDistancer::distance; distance_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeDistancer::distance; distance_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeDistancer::distance; distance_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeDistancer::distance; distance_matrix[BV_OBB][GEOM_HALFSPACE] = &BVHShapeDistancer::distance; */ distance_matrix[BV_RSS][GEOM_BOX] = &BVHShapeDistancer::distance; distance_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeDistancer::distance; distance_matrix[BV_RSS][GEOM_ELLIPSOID] = &BVHShapeDistancer::distance; distance_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeDistancer::distance; distance_matrix[BV_RSS][GEOM_CONE] = &BVHShapeDistancer::distance; distance_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeDistancer::distance; distance_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeDistancer::distance; distance_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeDistancer::distance; distance_matrix[BV_RSS][GEOM_HALFSPACE] = &BVHShapeDistancer::distance; /* KDOP distance not implemented */ /* distance_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeDistancer, Box, NarrowPhaseSolver>::distance; distance_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeDistancer, Sphere, NarrowPhaseSolver>::distance; distance_matrix[BV_KDOP16][GEOM_ELLIPSOID] = &BVHShapeDistancer, Ellipsoid, NarrowPhaseSolver>::distance; distance_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeDistancer, Capsule, NarrowPhaseSolver>::distance; distance_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeDistancer, Cone, NarrowPhaseSolver>::distance; distance_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeDistancer, Cylinder, NarrowPhaseSolver>::distance; distance_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeDistancer, Convex, NarrowPhaseSolver>::distance; distance_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeDistancer, Plane, NarrowPhaseSolver>::distance; distance_matrix[BV_KDOP16][GEOM_HALFSPACE] = &BVHShapeDistancer, Halfspace, NarrowPhaseSolver>::distance; distance_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeDistancer, Box, NarrowPhaseSolver>::distance; distance_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeDistancer, Sphere, NarrowPhaseSolver>::distance; distance_matrix[BV_KDOP18][GEOM_ELLIPSOID] = &BVHShapeDistancer, Ellipsoid, NarrowPhaseSolver>::distance; distance_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeDistancer, Capsule, NarrowPhaseSolver>::distance; distance_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeDistancer, Cone, NarrowPhaseSolver>::distance; distance_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeDistancer, Cylinder, NarrowPhaseSolver>::distance; distance_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeDistancer, Convex, NarrowPhaseSolver>::distance; distance_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeDistancer, Plane, NarrowPhaseSolver>::distance; distance_matrix[BV_KDOP18][GEOM_HALFSPACE] = &BVHShapeDistancer, Halfspace, NarrowPhaseSolver>::distance; distance_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeDistancer, Box, NarrowPhaseSolver>::distance; distance_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeDistancer, Sphere, NarrowPhaseSolver>::distance; distance_matrix[BV_KDOP24][GEOM_ELLIPSOID] = &BVHShapeDistancer, Ellipsoid, NarrowPhaseSolver>::distance; distance_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeDistancer, Capsule, NarrowPhaseSolver>::distance; distance_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeDistancer, Cone, NarrowPhaseSolver>::distance; distance_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeDistancer, Cylinder, NarrowPhaseSolver>::distance; distance_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeDistancer, Convex, NarrowPhaseSolver>::distance; distance_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeDistancer, Plane, NarrowPhaseSolver>::distance; distance_matrix[BV_KDOP24][GEOM_HALFSPACE] = &BVHShapeDistancer, Halfspace, NarrowPhaseSolver>::distance; */ distance_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeDistancer::distance; distance_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeDistancer::distance; distance_matrix[BV_kIOS][GEOM_ELLIPSOID] = &BVHShapeDistancer::distance; distance_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeDistancer::distance; distance_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeDistancer::distance; distance_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeDistancer::distance; distance_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeDistancer::distance; distance_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeDistancer::distance; distance_matrix[BV_kIOS][GEOM_HALFSPACE] = &BVHShapeDistancer::distance; distance_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeDistancer::distance; distance_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeDistancer::distance; distance_matrix[BV_OBBRSS][GEOM_ELLIPSOID] = &BVHShapeDistancer::distance; distance_matrix[BV_OBBRSS][GEOM_CAPSULE] = &BVHShapeDistancer::distance; distance_matrix[BV_OBBRSS][GEOM_CONE] = &BVHShapeDistancer::distance; distance_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeDistancer::distance; distance_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeDistancer::distance; distance_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeDistancer::distance; distance_matrix[BV_OBBRSS][GEOM_HALFSPACE] = &BVHShapeDistancer::distance; distance_matrix[BV_AABB][BV_AABB] = &BVHDistance; distance_matrix[BV_RSS][BV_RSS] = &BVHDistance; distance_matrix[BV_kIOS][BV_kIOS] = &BVHDistance; distance_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHDistance; #if FCL_HAVE_OCTOMAP distance_matrix[GEOM_OCTREE][GEOM_BOX] = &OcTreeShapeDistance; distance_matrix[GEOM_OCTREE][GEOM_SPHERE] = &OcTreeShapeDistance; distance_matrix[GEOM_OCTREE][GEOM_ELLIPSOID] = &OcTreeShapeDistance; distance_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &OcTreeShapeDistance; distance_matrix[GEOM_OCTREE][GEOM_CONE] = &OcTreeShapeDistance; distance_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &OcTreeShapeDistance; distance_matrix[GEOM_OCTREE][GEOM_CONVEX] = &OcTreeShapeDistance; distance_matrix[GEOM_OCTREE][GEOM_PLANE] = &OcTreeShapeDistance; distance_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &OcTreeShapeDistance; distance_matrix[GEOM_BOX][GEOM_OCTREE] = &ShapeOcTreeDistance; distance_matrix[GEOM_SPHERE][GEOM_OCTREE] = &ShapeOcTreeDistance; distance_matrix[GEOM_ELLIPSOID][GEOM_OCTREE] = &ShapeOcTreeDistance; distance_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &ShapeOcTreeDistance; distance_matrix[GEOM_CONE][GEOM_OCTREE] = &ShapeOcTreeDistance; distance_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &ShapeOcTreeDistance; distance_matrix[GEOM_CONVEX][GEOM_OCTREE] = &ShapeOcTreeDistance; distance_matrix[GEOM_PLANE][GEOM_OCTREE] = &ShapeOcTreeDistance; distance_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &ShapeOcTreeDistance; distance_matrix[GEOM_OCTREE][GEOM_OCTREE] = &OcTreeDistance; distance_matrix[GEOM_OCTREE][BV_AABB] = &OcTreeBVHDistance; distance_matrix[GEOM_OCTREE][BV_OBB] = &OcTreeBVHDistance; distance_matrix[GEOM_OCTREE][BV_RSS] = &OcTreeBVHDistance; distance_matrix[GEOM_OCTREE][BV_OBBRSS] = &OcTreeBVHDistance; distance_matrix[GEOM_OCTREE][BV_kIOS] = &OcTreeBVHDistance; distance_matrix[GEOM_OCTREE][BV_KDOP16] = &OcTreeBVHDistance, NarrowPhaseSolver>; distance_matrix[GEOM_OCTREE][BV_KDOP18] = &OcTreeBVHDistance, NarrowPhaseSolver>; distance_matrix[GEOM_OCTREE][BV_KDOP24] = &OcTreeBVHDistance, NarrowPhaseSolver>; distance_matrix[BV_AABB][GEOM_OCTREE] = &BVHOcTreeDistance; distance_matrix[BV_OBB][GEOM_OCTREE] = &BVHOcTreeDistance; distance_matrix[BV_RSS][GEOM_OCTREE] = &BVHOcTreeDistance; distance_matrix[BV_OBBRSS][GEOM_OCTREE] = &BVHOcTreeDistance; distance_matrix[BV_kIOS][GEOM_OCTREE] = &BVHOcTreeDistance; distance_matrix[BV_KDOP16][GEOM_OCTREE] = &BVHOcTreeDistance, NarrowPhaseSolver>; distance_matrix[BV_KDOP18][GEOM_OCTREE] = &BVHOcTreeDistance, NarrowPhaseSolver>; distance_matrix[BV_KDOP24][GEOM_OCTREE] = &BVHOcTreeDistance, NarrowPhaseSolver>; #endif } template struct DistanceFunctionMatrix; template struct DistanceFunctionMatrix; } fcl-0.5.0/src/intersect.cpp000066400000000000000000001460711274356571000155760ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/intersect.h" #include #include #include namespace fcl { const FCL_REAL PolySolver::NEAR_ZERO_THRESHOLD = 1e-9; bool PolySolver::isZero(FCL_REAL v) { return (v < NEAR_ZERO_THRESHOLD) && (v > -NEAR_ZERO_THRESHOLD); } bool PolySolver::cbrt(FCL_REAL v) { return powf(v, 1.0 / 3.0); } int PolySolver::solveLinear(FCL_REAL c[2], FCL_REAL s[1]) { if(isZero(c[1])) return 0; s[0] = - c[0] / c[1]; return 1; } int PolySolver::solveQuadric(FCL_REAL c[3], FCL_REAL s[2]) { FCL_REAL p, q, D; // make sure we have a d2 equation if(isZero(c[2])) return solveLinear(c, s); // normal for: x^2 + px + q p = c[1] / (2.0 * c[2]); q = c[0] / c[2]; D = p * p - q; if(isZero(D)) { // one FCL_REAL root s[0] = s[1] = -p; return 1; } if(D < 0.0) // no real root return 0; else { // two real roots FCL_REAL sqrt_D = sqrt(D); s[0] = sqrt_D - p; s[1] = -sqrt_D - p; return 2; } } int PolySolver::solveCubic(FCL_REAL c[4], FCL_REAL s[3]) { int i, num; FCL_REAL sub, A, B, C, sq_A, p, q, cb_p, D; const FCL_REAL ONE_OVER_THREE = 1 / 3.0; const FCL_REAL PI = 3.14159265358979323846; // make sure we have a d2 equation if(isZero(c[3])) return solveQuadric(c, s); // normalize the equation:x ^ 3 + Ax ^ 2 + Bx + C = 0 A = c[2] / c[3]; B = c[1] / c[3]; C = c[0] / c[3]; // substitute x = y - A / 3 to eliminate the quadratic term: x^3 + px + q = 0 sq_A = A * A; p = (-ONE_OVER_THREE * sq_A + B) * ONE_OVER_THREE; q = 0.5 * (2.0 / 27.0 * A * sq_A - ONE_OVER_THREE * A * B + C); // use Cardano's formula cb_p = p * p * p; D = q * q + cb_p; if(isZero(D)) { if(isZero(q)) { // one triple solution s[0] = 0.0; num = 1; } else { // one single and one FCL_REAL solution FCL_REAL u = cbrt(-q); s[0] = 2.0 * u; s[1] = -u; num = 2; } } else { if(D < 0.0) { // three real solutions FCL_REAL phi = ONE_OVER_THREE * acos(-q / sqrt(-cb_p)); FCL_REAL t = 2.0 * sqrt(-p); s[0] = t * cos(phi); s[1] = -t * cos(phi + PI / 3.0); s[2] = -t * cos(phi - PI / 3.0); num = 3; } else { // one real solution FCL_REAL sqrt_D = sqrt(D); FCL_REAL u = cbrt(sqrt_D + fabs(q)); if(q > 0.0) s[0] = - u + p / u ; else s[0] = u - p / u; num = 1; } } // re-substitute sub = ONE_OVER_THREE * A; for(i = 0; i < num; i++) s[i] -= sub; return num; } const FCL_REAL Intersect::EPSILON = 1e-5; const FCL_REAL Intersect::NEAR_ZERO_THRESHOLD = 1e-7; const FCL_REAL Intersect::CCD_RESOLUTION = 1e-7; bool Intersect::isZero(FCL_REAL v) { return (v < NEAR_ZERO_THRESHOLD) && (v > -NEAR_ZERO_THRESHOLD); } /// @brief data: only used for EE, return the intersect point bool Intersect::solveCubicWithIntervalNewton(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0, const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd, FCL_REAL& l, FCL_REAL& r, bool bVF, FCL_REAL coeffs[], Vec3f* data) { FCL_REAL v2[2]= {l*l,r*r}; FCL_REAL v[2]= {l,r}; FCL_REAL r_backup; unsigned char min3, min2, min1, max3, max2, max1; min3= *((unsigned char*)&coeffs[3]+7)>>7; max3=min3^1; min2= *((unsigned char*)&coeffs[2]+7)>>7; max2=min2^1; min1= *((unsigned char*)&coeffs[1]+7)>>7; max1=min1^1; // bound the cubic FCL_REAL minor = coeffs[3]*v2[min3]*v[min3]+coeffs[2]*v2[min2]+coeffs[1]*v[min1]+coeffs[0]; FCL_REAL major = coeffs[3]*v2[max3]*v[max3]+coeffs[2]*v2[max2]+coeffs[1]*v[max1]+coeffs[0]; if(major<0) return false; if(minor>0) return false; // starting here, the bounds have opposite values FCL_REAL m = 0.5 * (r + l); // bound the derivative FCL_REAL dminor = 3.0*coeffs[3]*v2[min3]+2.0*coeffs[2]*v[min2]+coeffs[1]; FCL_REAL dmajor = 3.0*coeffs[3]*v2[max3]+2.0*coeffs[2]*v[max2]+coeffs[1]; if((dminor > 0)||(dmajor < 0)) // we can use Newton { FCL_REAL m2 = m*m; FCL_REAL fm = coeffs[3]*m2*m+coeffs[2]*m2+coeffs[1]*m+coeffs[0]; FCL_REAL nl = m; FCL_REAL nu = m; if(fm>0) { nl-=(fm/dminor); nu-=(fm/dmajor); } else { nu-=(fm/dminor); nl-=(fm/dmajor); } //intersect with [l,r] if(nl>r) return false; if(nul) { if(nu 1) return false; *mub = (d3134 + d3412 * (*mua)) / d3434; if(*mub < 0 || *mub > 1) return false; *pa = p1 + p12 * (*mua); *pb = p3 + p34 * (*mub); return true; } bool Intersect::checkRootValidity_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0, const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp, FCL_REAL t) { return insideTriangle(a0 + va * t, b0 + vb * t, c0 + vc * t, p0 + vp * t); } bool Intersect::checkRootValidity_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0, const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd, FCL_REAL t, Vec3f* q_i) { Vec3f a = a0 + va * t; Vec3f b = b0 + vb * t; Vec3f c = c0 + vc * t; Vec3f d = d0 + vd * t; Vec3f p1, p2; FCL_REAL t_ab, t_cd; if(linelineIntersect(a, b, c, d, &p1, &p2, &t_ab, &t_cd)) { if(q_i) *q_i = p1; return true; } return false; } bool Intersect::checkRootValidity_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0, const Vec3f& va, const Vec3f& vb, const Vec3f& vp, FCL_REAL t) { return insideLineSegment(a0 + va * t, b0 + vb * t, p0 + vp * t); } bool Intersect::solveSquare(FCL_REAL a, FCL_REAL b, FCL_REAL c, const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0, const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd, bool bVF, FCL_REAL* ret) { FCL_REAL discriminant = b * b - 4 * a * c; if(discriminant < 0) return false; FCL_REAL sqrt_dis = sqrt(discriminant); FCL_REAL r1 = (-b + sqrt_dis) / (2 * a); bool v1 = (r1 >= 0.0 && r1 <= 1.0) ? ((bVF) ? checkRootValidity_VF(a0, b0, c0, d0, va, vb, vc, vd, r1) : checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r1)) : false; FCL_REAL r2 = (-b - sqrt_dis) / (2 * a); bool v2 = (r2 >= 0.0 && r2 <= 1.0) ? ((bVF) ? checkRootValidity_VF(a0, b0, c0, d0, va, vb, vc, vd, r2) : checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r2)) : false; if(v1 && v2) { *ret = (r1 > r2) ? r2 : r1; return true; } if(v1) { *ret = r1; return true; } if(v2) { *ret = r2; return true; } return false; } bool Intersect::solveSquare(FCL_REAL a, FCL_REAL b, FCL_REAL c, const Vec3f& a0, const Vec3f& b0, const Vec3f& p0, const Vec3f& va, const Vec3f& vb, const Vec3f& vp) { if(isZero(a)) { FCL_REAL t = -c/b; return (t >= 0 && t <= 1) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, t) : false; } FCL_REAL discriminant = b*b-4*a*c; if(discriminant < 0) return false; FCL_REAL sqrt_dis = sqrt(discriminant); FCL_REAL r1 = (-b+sqrt_dis) / (2 * a); bool v1 = (r1 >= 0.0 && r1 <= 1.0) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, r1) : false; if(v1) return true; FCL_REAL r2 = (-b-sqrt_dis) / (2 * a); bool v2 = (r2 >= 0.0 && r2 <= 1.0) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, r2) : false; return v2; } /// @brief Compute the cubic coefficients for VF case /// See Paper "Interactive Continuous Collision Detection between Deformable Models using Connectivity-Based Culling", Equation 1. void Intersect::computeCubicCoeff_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0, const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp, FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d) { Vec3f vavb = vb - va; Vec3f vavc = vc - va; Vec3f vavp = vp - va; Vec3f a0b0 = b0 - a0; Vec3f a0c0 = c0 - a0; Vec3f a0p0 = p0 - a0; Vec3f vavb_cross_vavc = vavb.cross(vavc); Vec3f vavb_cross_a0c0 = vavb.cross(a0c0); Vec3f a0b0_cross_vavc = a0b0.cross(vavc); Vec3f a0b0_cross_a0c0 = a0b0.cross(a0c0); *a = vavp.dot(vavb_cross_vavc); *b = a0p0.dot(vavb_cross_vavc) + vavp.dot(vavb_cross_a0c0 + a0b0_cross_vavc); *c = vavp.dot(a0b0_cross_a0c0) + a0p0.dot(vavb_cross_a0c0 + a0b0_cross_vavc); *d = a0p0.dot(a0b0_cross_a0c0); } void Intersect::computeCubicCoeff_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0, const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd, FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d) { Vec3f vavb = vb - va; Vec3f vcvd = vd - vc; Vec3f vavc = vc - va; Vec3f c0d0 = d0 - c0; Vec3f a0b0 = b0 - a0; Vec3f a0c0 = c0 - a0; Vec3f vavb_cross_vcvd = vavb.cross(vcvd); Vec3f vavb_cross_c0d0 = vavb.cross(c0d0); Vec3f a0b0_cross_vcvd = a0b0.cross(vcvd); Vec3f a0b0_cross_c0d0 = a0b0.cross(c0d0); *a = vavc.dot(vavb_cross_vcvd); *b = a0c0.dot(vavb_cross_vcvd) + vavc.dot(vavb_cross_c0d0 + a0b0_cross_vcvd); *c = vavc.dot(a0b0_cross_c0d0) + a0c0.dot(vavb_cross_c0d0 + a0b0_cross_vcvd); *d = a0c0.dot(a0b0_cross_c0d0); } void Intersect::computeCubicCoeff_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0, const Vec3f& va, const Vec3f& vb, const Vec3f& vp, const Vec3f& L, FCL_REAL* a, FCL_REAL* b, FCL_REAL* c) { Vec3f vbva = va - vb; Vec3f vbvp = vp - vb; Vec3f b0a0 = a0 - b0; Vec3f b0p0 = p0 - b0; Vec3f L_cross_vbvp = L.cross(vbvp); Vec3f L_cross_b0p0 = L.cross(b0p0); *a = L_cross_vbvp.dot(vbva); *b = L_cross_vbvp.dot(b0a0) + L_cross_b0p0.dot(vbva); *c = L_cross_b0p0.dot(b0a0); } bool Intersect::intersect_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0, const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1, FCL_REAL* collision_time, Vec3f* p_i, bool useNewton) { *collision_time = 2.0; Vec3f vp, va, vb, vc; vp = p1 - p0; va = a1 - a0; vb = b1 - b0; vc = c1 - c0; FCL_REAL a, b, c, d; computeCubicCoeff_VF(a0, b0, c0, p0, va, vb, vc, vp, &a, &b, &c, &d); if(isZero(a) && isZero(b) && isZero(c) && isZero(d)) { return false; } /// if(isZero(a)) /// { /// return solveSquare(b, c, d, a0, b0, c0, p0, va, vb, vc, vp, true, collision_time); /// } FCL_REAL coeffs[4]; coeffs[3] = a, coeffs[2] = b, coeffs[1] = c, coeffs[0] = d; if(useNewton) { FCL_REAL l = 0; FCL_REAL r = 1; if(solveCubicWithIntervalNewton(a0, b0, c0, p0, va, vb, vc, vp, l, r, true, coeffs)) { *collision_time = 0.5 * (l + r); } } else { FCL_REAL roots[3]; int num = PolySolver::solveCubic(coeffs, roots); for(int i = 0; i < num; ++i) { FCL_REAL r = roots[i]; if(r < 0 || r > 1) continue; if(checkRootValidity_VF(a0, b0, c0, p0, va, vb, vc, vp, r)) { *collision_time = r; break; } } } if(*collision_time > 1) { return false; } *p_i = vp * (*collision_time) + p0; return true; } bool Intersect::intersect_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0, const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1, FCL_REAL* collision_time, Vec3f* p_i, bool useNewton) { *collision_time = 2.0; Vec3f va, vb, vc, vd; va = a1 - a0; vb = b1 - b0; vc = c1 - c0; vd = d1 - d0; FCL_REAL a, b, c, d; computeCubicCoeff_EE(a0, b0, c0, d0, va, vb, vc, vd, &a, &b, &c, &d); if(isZero(a) && isZero(b) && isZero(c) && isZero(d)) { return false; } /// if(isZero(a)) /// { /// return solveSquare(b, c, d, a0, b0, c0, d0, va, vb, vc, vd, collision_time, false); /// } FCL_REAL coeffs[4]; coeffs[3] = a, coeffs[2] = b, coeffs[1] = c, coeffs[0] = d; if(useNewton) { FCL_REAL l = 0; FCL_REAL r = 1; if(solveCubicWithIntervalNewton(a0, b0, c0, d0, va, vb, vc, vd, l, r, false, coeffs, p_i)) { *collision_time = (l + r) * 0.5; } } else { FCL_REAL roots[3]; int num = PolySolver::solveCubic(coeffs, roots); for(int i = 0; i < num; ++i) { FCL_REAL r = roots[i]; if(r < 0 || r > 1) continue; if(checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r, p_i)) { *collision_time = r; break; } } } if(*collision_time > 1) { return false; } return true; } bool Intersect::intersect_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0, const Vec3f& a1, const Vec3f& b1, const Vec3f& p1, const Vec3f& L) { Vec3f va, vb, vp; va = a1 - a0; vb = b1 - b0; vp = p1 - p0; FCL_REAL a, b, c; computeCubicCoeff_VE(a0, b0, p0, va, vb, vp, L, &a, &b, &c); if(isZero(a) && isZero(b) && isZero(c)) return true; return solveSquare(a, b, c, a0, b0, p0, va, vb, vp); } /// @brief Prefilter for intersection, works for both VF and EE bool Intersect::intersectPreFiltering(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0, const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1) { Vec3f n0 = (b0 - a0).cross(c0 - a0); Vec3f n1 = (b1 - a1).cross(c1 - a1); Vec3f a0a1 = a1 - a0; Vec3f b0b1 = b1 - b0; Vec3f c0c1 = c1 - c0; Vec3f delta = (b0b1 - a0a1).cross(c0c1 - a0a1); Vec3f nx = (n0 + n1 - delta) * 0.5; Vec3f a0d0 = d0 - a0; Vec3f a1d1 = d1 - a1; FCL_REAL A = n0.dot(a0d0); FCL_REAL B = n1.dot(a1d1); FCL_REAL C = nx.dot(a0d0); FCL_REAL D = nx.dot(a1d1); FCL_REAL E = n1.dot(a0d0); FCL_REAL F = n0.dot(a1d1); if(A > 0 && B > 0 && (2*C +F) > 0 && (2*D+E) > 0) return false; if(A < 0 && B < 0 && (2*C +F) < 0 && (2*D+E) < 0) return false; return true; } bool Intersect::intersect_VF_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0, const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1, FCL_REAL* collision_time, Vec3f* p_i, bool useNewton) { if(intersectPreFiltering(a0, b0, c0, p0, a1, b1, c1, p1)) { return intersect_VF(a0, b0, c0, p0, a1, b1, c1, p1, collision_time, p_i, useNewton); } else return false; } bool Intersect::intersect_EE_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0, const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1, FCL_REAL* collision_time, Vec3f* p_i, bool useNewton) { if(intersectPreFiltering(a0, b0, c0, d0, a1, b1, c1, d1)) { return intersect_EE(a0, b0, c0, d0, a1, b1, c1, d1, collision_time, p_i, useNewton); } else return false; } bool Intersect::intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3, const Matrix3f& R, const Vec3f& T, Vec3f* contact_points, unsigned int* num_contact_points, FCL_REAL* penetration_depth, Vec3f* normal) { Vec3f Q1_ = R * Q1 + T; Vec3f Q2_ = R * Q2 + T; Vec3f Q3_ = R * Q3 + T; return intersect_Triangle(P1, P2, P3, Q1_, Q2_, Q3_, contact_points, num_contact_points, penetration_depth, normal); } bool Intersect::intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3, const Transform3f& tf, Vec3f* contact_points, unsigned int* num_contact_points, FCL_REAL* penetration_depth, Vec3f* normal) { Vec3f Q1_ = tf.transform(Q1); Vec3f Q2_ = tf.transform(Q2); Vec3f Q3_ = tf.transform(Q3); return intersect_Triangle(P1, P2, P3, Q1_, Q2_, Q3_, contact_points, num_contact_points, penetration_depth, normal); } #if ODE_STYLE bool Intersect::intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3, Vec3f* contact_points, unsigned int* num_contact_points, FCL_REAL* penetration_depth, Vec3f* normal) { Vec3f n1; FCL_REAL t1; bool b1 = buildTrianglePlane(P1, P2, P3, &n1, &t1); if(!b1) return false; Vec3f n2; FCL_REAL t2; bool b2 = buildTrianglePlane(Q1, Q2, Q3, &n2, &t2); if(!b2) return false; if(sameSideOfPlane(P1, P2, P3, n2, t2)) return false; if(sameSideOfPlane(Q1, Q2, Q3, n1, t1)) return false; Vec3f clipped_points1[MAX_TRIANGLE_CLIPS]; unsigned int num_clipped_points1 = 0; Vec3f clipped_points2[MAX_TRIANGLE_CLIPS]; unsigned int num_clipped_points2 = 0; Vec3f deepest_points1[MAX_TRIANGLE_CLIPS]; unsigned int num_deepest_points1 = 0; Vec3f deepest_points2[MAX_TRIANGLE_CLIPS]; unsigned int num_deepest_points2 = 0; FCL_REAL penetration_depth1 = -1, penetration_depth2 = -1; clipTriangleByTriangleAndEdgePlanes(Q1, Q2, Q3, P1, P2, P3, n1, t1, clipped_points2, &num_clipped_points2); if(num_clipped_points2 == 0) return false; computeDeepestPoints(clipped_points2, num_clipped_points2, n1, t1, &penetration_depth2, deepest_points2, &num_deepest_points2); if(num_deepest_points2 == 0) return false; clipTriangleByTriangleAndEdgePlanes(P1, P2, P3, Q1, Q2, Q3, n2, t2, clipped_points1, &num_clipped_points1); if(num_clipped_points1 == 0) return false; computeDeepestPoints(clipped_points1, num_clipped_points1, n2, t2, &penetration_depth1, deepest_points1, &num_deepest_points1); if(num_deepest_points1 == 0) return false; /// Return contact information if(contact_points && num_contact_points && penetration_depth && normal) { if(penetration_depth1 > penetration_depth2) { *num_contact_points = num_deepest_points2; for(unsigned int i = 0; i < num_deepest_points2; ++i) { contact_points[i] = deepest_points2[i]; } *normal = n1; *penetration_depth = penetration_depth2; } else { *num_contact_points = num_deepest_points1; for(unsigned int i = 0; i < num_deepest_points1; ++i) { contact_points[i] = deepest_points1[i]; } *normal = -n2; *penetration_depth = penetration_depth1; } } return true; } #else bool Intersect::intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3, Vec3f* contact_points, unsigned int* num_contact_points, FCL_REAL* penetration_depth, Vec3f* normal) { Vec3f p1 = P1 - P1; Vec3f p2 = P2 - P1; Vec3f p3 = P3 - P1; Vec3f q1 = Q1 - P1; Vec3f q2 = Q2 - P1; Vec3f q3 = Q3 - P1; Vec3f e1 = p2 - p1; Vec3f e2 = p3 - p2; Vec3f n1 = e1.cross(e2); if (!project6(n1, p1, p2, p3, q1, q2, q3)) return false; Vec3f f1 = q2 - q1; Vec3f f2 = q3 - q2; Vec3f m1 = f1.cross(f2); if (!project6(m1, p1, p2, p3, q1, q2, q3)) return false; Vec3f ef11 = e1.cross(f1); if (!project6(ef11, p1, p2, p3, q1, q2, q3)) return false; Vec3f ef12 = e1.cross(f2); if (!project6(ef12, p1, p2, p3, q1, q2, q3)) return false; Vec3f f3 = q1 - q3; Vec3f ef13 = e1.cross(f3); if (!project6(ef13, p1, p2, p3, q1, q2, q3)) return false; Vec3f ef21 = e2.cross(f1); if (!project6(ef21, p1, p2, p3, q1, q2, q3)) return false; Vec3f ef22 = e2.cross(f2); if (!project6(ef22, p1, p2, p3, q1, q2, q3)) return false; Vec3f ef23 = e2.cross(f3); if (!project6(ef23, p1, p2, p3, q1, q2, q3)) return false; Vec3f e3 = p1 - p3; Vec3f ef31 = e3.cross(f1); if (!project6(ef31, p1, p2, p3, q1, q2, q3)) return false; Vec3f ef32 = e3.cross(f2); if (!project6(ef32, p1, p2, p3, q1, q2, q3)) return false; Vec3f ef33 = e3.cross(f3); if (!project6(ef33, p1, p2, p3, q1, q2, q3)) return false; Vec3f g1 = e1.cross(n1); if (!project6(g1, p1, p2, p3, q1, q2, q3)) return false; Vec3f g2 = e2.cross(n1); if (!project6(g2, p1, p2, p3, q1, q2, q3)) return false; Vec3f g3 = e3.cross(n1); if (!project6(g3, p1, p2, p3, q1, q2, q3)) return false; Vec3f h1 = f1.cross(m1); if (!project6(h1, p1, p2, p3, q1, q2, q3)) return false; Vec3f h2 = f2.cross(m1); if (!project6(h2, p1, p2, p3, q1, q2, q3)) return false; Vec3f h3 = f3.cross(m1); if (!project6(h3, p1, p2, p3, q1, q2, q3)) return false; if(contact_points && num_contact_points && penetration_depth && normal) { Vec3f n1, n2; FCL_REAL t1, t2; buildTrianglePlane(P1, P2, P3, &n1, &t1); buildTrianglePlane(Q1, Q2, Q3, &n2, &t2); Vec3f deepest_points1[3]; unsigned int num_deepest_points1 = 0; Vec3f deepest_points2[3]; unsigned int num_deepest_points2 = 0; FCL_REAL penetration_depth1, penetration_depth2; Vec3f P[3] = {P1, P2, P3}; Vec3f Q[3] = {Q1, Q2, Q3}; computeDeepestPoints(Q, 3, n1, t1, &penetration_depth2, deepest_points2, &num_deepest_points2); computeDeepestPoints(P, 3, n2, t2, &penetration_depth1, deepest_points1, &num_deepest_points1); if(penetration_depth1 > penetration_depth2) { *num_contact_points = std::min(num_deepest_points2, (unsigned int)2); for(unsigned int i = 0; i < *num_contact_points; ++i) { contact_points[i] = deepest_points2[i]; } *normal = n1; *penetration_depth = penetration_depth2; } else { *num_contact_points = std::min(num_deepest_points1, (unsigned int)2); for(unsigned int i = 0; i < *num_contact_points; ++i) { contact_points[i] = deepest_points1[i]; } *normal = -n2; *penetration_depth = penetration_depth1; } } return true; } #endif void Intersect::computeDeepestPoints(Vec3f* clipped_points, unsigned int num_clipped_points, const Vec3f& n, FCL_REAL t, FCL_REAL* penetration_depth, Vec3f* deepest_points, unsigned int* num_deepest_points) { *num_deepest_points = 0; FCL_REAL max_depth = -std::numeric_limits::max(); unsigned int num_deepest_points_ = 0; unsigned int num_neg = 0; unsigned int num_pos = 0; unsigned int num_zero = 0; for(unsigned int i = 0; i < num_clipped_points; ++i) { FCL_REAL dist = -distanceToPlane(n, t, clipped_points[i]); if(dist > EPSILON) num_pos++; else if(dist < -EPSILON) num_neg++; else num_zero++; if(dist > max_depth) { max_depth = dist; num_deepest_points_ = 1; deepest_points[num_deepest_points_ - 1] = clipped_points[i]; } else if(dist + 1e-6 >= max_depth) { num_deepest_points_++; deepest_points[num_deepest_points_ - 1] = clipped_points[i]; } } if(max_depth < -EPSILON) num_deepest_points_ = 0; if(num_zero == 0 && ((num_neg == 0) || (num_pos == 0))) num_deepest_points_ = 0; *penetration_depth = max_depth; *num_deepest_points = num_deepest_points_; } void Intersect::clipTriangleByTriangleAndEdgePlanes(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, const Vec3f& t1, const Vec3f& t2, const Vec3f& t3, const Vec3f& tn, FCL_REAL to, Vec3f clipped_points[], unsigned int* num_clipped_points, bool clip_triangle) { *num_clipped_points = 0; Vec3f temp_clip[MAX_TRIANGLE_CLIPS]; Vec3f temp_clip2[MAX_TRIANGLE_CLIPS]; unsigned int num_temp_clip = 0; unsigned int num_temp_clip2 = 0; Vec3f v[3] = {v1, v2, v3}; Vec3f plane_n; FCL_REAL plane_dist; if(buildEdgePlane(t1, t2, tn, &plane_n, &plane_dist)) { clipPolygonByPlane(v, 3, plane_n, plane_dist, temp_clip, &num_temp_clip); if(num_temp_clip > 0) { if(buildEdgePlane(t2, t3, tn, &plane_n, &plane_dist)) { clipPolygonByPlane(temp_clip, num_temp_clip, plane_n, plane_dist, temp_clip2, &num_temp_clip2); if(num_temp_clip2 > 0) { if(buildEdgePlane(t3, t1, tn, &plane_n, &plane_dist)) { if(clip_triangle) { num_temp_clip = 0; clipPolygonByPlane(temp_clip2, num_temp_clip2, plane_n, plane_dist, temp_clip, &num_temp_clip); if(num_temp_clip > 0) { clipPolygonByPlane(temp_clip, num_temp_clip, tn, to, clipped_points, num_clipped_points); } } else { clipPolygonByPlane(temp_clip2, num_temp_clip2, plane_n, plane_dist, clipped_points, num_clipped_points); } } } } } } } void Intersect::clipPolygonByPlane(Vec3f* polygon_points, unsigned int num_polygon_points, const Vec3f& n, FCL_REAL t, Vec3f clipped_points[], unsigned int* num_clipped_points) { *num_clipped_points = 0; unsigned int num_clipped_points_ = 0; unsigned int vi; unsigned int prev_classify = 2; unsigned int classify; for(unsigned int i = 0; i <= num_polygon_points; ++i) { vi = (i % num_polygon_points); FCL_REAL d = distanceToPlane(n, t, polygon_points[i]); classify = ((d > EPSILON) ? 1 : 0); if(classify == 0) { if(prev_classify == 1) { if(num_clipped_points_ < MAX_TRIANGLE_CLIPS) { Vec3f tmp; clipSegmentByPlane(polygon_points[i - 1], polygon_points[vi], n, t, &tmp); if(num_clipped_points_ > 0) { if((tmp - clipped_points[num_clipped_points_ - 1]).sqrLength() > EPSILON) { clipped_points[num_clipped_points_] = tmp; num_clipped_points_++; } } else { clipped_points[num_clipped_points_] = tmp; num_clipped_points_++; } } } if(num_clipped_points_ < MAX_TRIANGLE_CLIPS && i < num_polygon_points) { clipped_points[num_clipped_points_] = polygon_points[vi]; num_clipped_points_++; } } else { if(prev_classify == 0) { if(num_clipped_points_ < MAX_TRIANGLE_CLIPS) { Vec3f tmp; clipSegmentByPlane(polygon_points[i - 1], polygon_points[vi], n, t, &tmp); if(num_clipped_points_ > 0) { if((tmp - clipped_points[num_clipped_points_ - 1]).sqrLength() > EPSILON) { clipped_points[num_clipped_points_] = tmp; num_clipped_points_++; } } else { clipped_points[num_clipped_points_] = tmp; num_clipped_points_++; } } } } prev_classify = classify; } if(num_clipped_points_ > 2) { if((clipped_points[0] - clipped_points[num_clipped_points_ - 1]).sqrLength() < EPSILON) { num_clipped_points_--; } } *num_clipped_points = num_clipped_points_; } void Intersect::clipSegmentByPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& n, FCL_REAL t, Vec3f* clipped_point) { FCL_REAL dist1 = distanceToPlane(n, t, v1); Vec3f tmp = v2 - v1; FCL_REAL dist2 = tmp.dot(n); *clipped_point = tmp * (-dist1 / dist2) + v1; } FCL_REAL Intersect::distanceToPlane(const Vec3f& n, FCL_REAL t, const Vec3f& v) { return n.dot(v) - t; } bool Intersect::buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, Vec3f* n, FCL_REAL* t) { Vec3f n_ = (v2 - v1).cross(v3 - v1); bool can_normalize = false; n_.normalize(&can_normalize); if(can_normalize) { *n = n_; *t = n_.dot(v1); return true; } return false; } bool Intersect::buildEdgePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& tn, Vec3f* n, FCL_REAL* t) { Vec3f n_ = (v2 - v1).cross(tn); bool can_normalize = false; n_.normalize(&can_normalize); if(can_normalize) { *n = n_; *t = n_.dot(v1); return true; } return false; } bool Intersect::sameSideOfPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, const Vec3f& n, FCL_REAL t) { FCL_REAL dist1 = distanceToPlane(n, t, v1); FCL_REAL dist2 = dist1 * distanceToPlane(n, t, v2); FCL_REAL dist3 = dist1 * distanceToPlane(n, t, v3); if((dist2 > 0) && (dist3 > 0)) return true; return false; } int Intersect::project6(const Vec3f& ax, const Vec3f& p1, const Vec3f& p2, const Vec3f& p3, const Vec3f& q1, const Vec3f& q2, const Vec3f& q3) { FCL_REAL P1 = ax.dot(p1); FCL_REAL P2 = ax.dot(p2); FCL_REAL P3 = ax.dot(p3); FCL_REAL Q1 = ax.dot(q1); FCL_REAL Q2 = ax.dot(q2); FCL_REAL Q3 = ax.dot(q3); FCL_REAL mn1 = std::min(P1, std::min(P2, P3)); FCL_REAL mx2 = std::max(Q1, std::max(Q2, Q3)); if(mn1 > mx2) return 0; FCL_REAL mx1 = std::max(P1, std::max(P2, P3)); FCL_REAL mn2 = std::min(Q1, std::min(Q2, Q3)); if(mn2 > mx1) return 0; return 1; } void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, const Vec3f& B, Vec3f& VEC, Vec3f& X, Vec3f& Y) { Vec3f T; FCL_REAL A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T; Vec3f TMP; T = Q - P; A_dot_A = A.dot(A); B_dot_B = B.dot(B); A_dot_B = A.dot(B); A_dot_T = A.dot(T); B_dot_T = B.dot(T); // t parameterizes ray P,A // u parameterizes ray Q,B FCL_REAL t, u; // compute t for the closest point on ray P,A to // ray Q,B FCL_REAL denom = A_dot_A*B_dot_B - A_dot_B*A_dot_B; t = (A_dot_T*B_dot_B - B_dot_T*A_dot_B) / denom; // clamp result so t is on the segment P,A if((t < 0) || std::isnan(t)) t = 0; else if(t > 1) t = 1; // find u for point on ray Q,B closest to point at t u = (t*A_dot_B - B_dot_T) / B_dot_B; // if u is on segment Q,B, t and u correspond to // closest points, otherwise, clamp u, recompute and // clamp t if((u <= 0) || std::isnan(u)) { Y = Q; t = A_dot_T / A_dot_A; if((t <= 0) || std::isnan(t)) { X = P; VEC = Q - P; } else if(t >= 1) { X = P + A; VEC = Q - X; } else { X = P + A * t; TMP = T.cross(A); VEC = A.cross(TMP); } } else if (u >= 1) { Y = Q + B; t = (A_dot_B + A_dot_T) / A_dot_A; if((t <= 0) || std::isnan(t)) { X = P; VEC = Y - P; } else if(t >= 1) { X = P + A; VEC = Y - X; } else { X = P + A * t; T = Y - P; TMP = T.cross(A); VEC= A.cross(TMP); } } else { Y = Q + B * u; if((t <= 0) || std::isnan(t)) { X = P; TMP = T.cross(B); VEC = B.cross(TMP); } else if(t >= 1) { X = P + A; T = Q - X; TMP = T.cross(B); VEC = B.cross(TMP); } else { X = P + A * t; VEC = A.cross(B); if(VEC.dot(T) < 0) { VEC = VEC * (-1); } } } } FCL_REAL TriangleDistance::triDistance(const Vec3f S[3], const Vec3f T[3], Vec3f& P, Vec3f& Q) { // Compute vectors along the 6 sides Vec3f Sv[3]; Vec3f Tv[3]; Vec3f VEC; Sv[0] = S[1] - S[0]; Sv[1] = S[2] - S[1]; Sv[2] = S[0] - S[2]; Tv[0] = T[1] - T[0]; Tv[1] = T[2] - T[1]; Tv[2] = T[0] - T[2]; // For each edge pair, the vector connecting the closest points // of the edges defines a slab (parallel planes at head and tail // enclose the slab). If we can show that the off-edge vertex of // each triangle is outside of the slab, then the closest points // of the edges are the closest points for the triangles. // Even if these tests fail, it may be helpful to know the closest // points found, and whether the triangles were shown disjoint Vec3f V, Z, minP, minQ; FCL_REAL mindd; int shown_disjoint = 0; mindd = (S[0] - T[0]).sqrLength() + 1; // Set first minimum safely high for(int i = 0; i < 3; ++i) { for(int j = 0; j < 3; ++j) { // Find closest points on edges i & j, plus the // vector (and distance squared) between these points segPoints(S[i], Sv[i], T[j], Tv[j], VEC, P, Q); V = Q - P; FCL_REAL dd = V.dot(V); // Verify this closest point pair only if the distance // squared is less than the minimum found thus far. if(dd <= mindd) { minP = P; minQ = Q; mindd = dd; Z = S[(i+2)%3] - P; FCL_REAL a = Z.dot(VEC); Z = T[(j+2)%3] - Q; FCL_REAL b = Z.dot(VEC); if((a <= 0) && (b >= 0)) return sqrt(dd); FCL_REAL p = V.dot(VEC); if(a < 0) a = 0; if(b > 0) b = 0; if((p - a + b) > 0) shown_disjoint = 1; } } } // No edge pairs contained the closest points. // either: // 1. one of the closest points is a vertex, and the // other point is interior to a face. // 2. the triangles are overlapping. // 3. an edge of one triangle is parallel to the other's face. If // cases 1 and 2 are not true, then the closest points from the 9 // edge pairs checks above can be taken as closest points for the // triangles. // 4. possibly, the triangles were degenerate. When the // triangle points are nearly colinear or coincident, one // of above tests might fail even though the edges tested // contain the closest points. // First check for case 1 Vec3f Sn; FCL_REAL Snl; Sn = Sv[0].cross(Sv[1]); // Compute normal to S triangle Snl = Sn.dot(Sn); // Compute square of length of normal // If cross product is long enough, if(Snl > 1e-15) { // Get projection lengths of T points Vec3f Tp; V = S[0] - T[0]; Tp[0] = V.dot(Sn); V = S[0] - T[1]; Tp[1] = V.dot(Sn); V = S[0] - T[2]; Tp[2] = V.dot(Sn); // If Sn is a separating direction, // find point with smallest projection int point = -1; if((Tp[0] > 0) && (Tp[1] > 0) && (Tp[2] > 0)) { if(Tp[0] < Tp[1]) point = 0; else point = 1; if(Tp[2] < Tp[point]) point = 2; } else if((Tp[0] < 0) && (Tp[1] < 0) && (Tp[2] < 0)) { if(Tp[0] > Tp[1]) point = 0; else point = 1; if(Tp[2] > Tp[point]) point = 2; } // If Sn is a separating direction, if(point >= 0) { shown_disjoint = 1; // Test whether the point found, when projected onto the // other triangle, lies within the face. V = T[point] - S[0]; Z = Sn.cross(Sv[0]); if(V.dot(Z) > 0) { V = T[point] - S[1]; Z = Sn.cross(Sv[1]); if(V.dot(Z) > 0) { V = T[point] - S[2]; Z = Sn.cross(Sv[2]); if(V.dot(Z) > 0) { // T[point] passed the test - it's a closest point for // the T triangle; the other point is on the face of S P = T[point] + Sn * (Tp[point] / Snl); Q = T[point]; return (P - Q).length(); } } } } } Vec3f Tn; FCL_REAL Tnl; Tn = Tv[0].cross(Tv[1]); Tnl = Tn.dot(Tn); if(Tnl > 1e-15) { Vec3f Sp; V = T[0] - S[0]; Sp[0] = V.dot(Tn); V = T[0] - S[1]; Sp[1] = V.dot(Tn); V = T[0] - S[2]; Sp[2] = V.dot(Tn); int point = -1; if((Sp[0] > 0) && (Sp[1] > 0) && (Sp[2] > 0)) { if(Sp[0] < Sp[1]) point = 0; else point = 1; if(Sp[2] < Sp[point]) point = 2; } else if((Sp[0] < 0) && (Sp[1] < 0) && (Sp[2] < 0)) { if(Sp[0] > Sp[1]) point = 0; else point = 1; if(Sp[2] > Sp[point]) point = 2; } if(point >= 0) { shown_disjoint = 1; V = S[point] - T[0]; Z = Tn.cross(Tv[0]); if(V.dot(Z) > 0) { V = S[point] - T[1]; Z = Tn.cross(Tv[1]); if(V.dot(Z) > 0) { V = S[point] - T[2]; Z = Tn.cross(Tv[2]); if(V.dot(Z) > 0) { P = S[point]; Q = S[point] + Tn * (Sp[point] / Tnl); return (P - Q).length(); } } } } } // Case 1 can't be shown. // If one of these tests showed the triangles disjoint, // we assume case 3 or 4, otherwise we conclude case 2, // that the triangles overlap. if(shown_disjoint) { P = minP; Q = minQ; return sqrt(mindd); } else return 0; } FCL_REAL TriangleDistance::triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3, const Vec3f& T1, const Vec3f& T2, const Vec3f& T3, Vec3f& P, Vec3f& Q) { Vec3f S[3]; Vec3f T[3]; S[0] = S1; S[1] = S2; S[2] = S3; T[0] = T1; T[1] = T2; T[2] = T3; return triDistance(S, T, P, Q); } FCL_REAL TriangleDistance::triDistance(const Vec3f S[3], const Vec3f T[3], const Matrix3f& R, const Vec3f& Tl, Vec3f& P, Vec3f& Q) { Vec3f T_transformed[3]; T_transformed[0] = R * T[0] + Tl; T_transformed[1] = R * T[1] + Tl; T_transformed[2] = R * T[2] + Tl; return triDistance(S, T_transformed, P, Q); } FCL_REAL TriangleDistance::triDistance(const Vec3f S[3], const Vec3f T[3], const Transform3f& tf, Vec3f& P, Vec3f& Q) { Vec3f T_transformed[3]; T_transformed[0] = tf.transform(T[0]); T_transformed[1] = tf.transform(T[1]); T_transformed[2] = tf.transform(T[2]); return triDistance(S, T_transformed, P, Q); } FCL_REAL TriangleDistance::triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3, const Vec3f& T1, const Vec3f& T2, const Vec3f& T3, const Matrix3f& R, const Vec3f& Tl, Vec3f& P, Vec3f& Q) { Vec3f T1_transformed = R * T1 + Tl; Vec3f T2_transformed = R * T2 + Tl; Vec3f T3_transformed = R * T3 + Tl; return triDistance(S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q); } FCL_REAL TriangleDistance::triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3, const Vec3f& T1, const Vec3f& T2, const Vec3f& T3, const Transform3f& tf, Vec3f& P, Vec3f& Q) { Vec3f T1_transformed = tf.transform(T1); Vec3f T2_transformed = tf.transform(T2); Vec3f T3_transformed = tf.transform(T3); return triDistance(S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q); } Project::ProjectResult Project::projectLine(const Vec3f& a, const Vec3f& b, const Vec3f& p) { ProjectResult res; const Vec3f d = b - a; const FCL_REAL l = d.sqrLength(); if(l > 0) { const FCL_REAL t = (p - a).dot(d); res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l)); res.parameterization[0] = 1 - res.parameterization[1]; if(t >= l) { res.sqr_distance = (p - b).sqrLength(); res.encode = 2; /* 0x10 */ } else if(t <= 0) { res.sqr_distance = (p - a).sqrLength(); res.encode = 1; /* 0x01 */ } else { res.sqr_distance = (a + d * res.parameterization[1] - p).sqrLength(); res.encode = 3; /* 0x00 */ } } return res; } Project::ProjectResult Project::projectTriangle(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& p) { ProjectResult res; static const size_t nexti[3] = {1, 2, 0}; const Vec3f* vt[] = {&a, &b, &c}; const Vec3f dl[] = {a - b, b - c, c - a}; const Vec3f& n = dl[0].cross(dl[1]); const FCL_REAL l = n.sqrLength(); if(l > 0) { FCL_REAL mindist = -1; for(size_t i = 0; i < 3; ++i) { if((*vt[i] - p).dot(dl[i].cross(n)) > 0) // origin is to the outside part of the triangle edge, then the optimal can only be on the edge { size_t j = nexti[i]; ProjectResult res_line = projectLine(*vt[i], *vt[j], p); if(mindist < 0 || res_line.sqr_distance < mindist) { mindist = res_line.sqr_distance; res.encode = static_cast(((res_line.encode&1)?1< 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng is false, then the last vertex in the tetrahedron does not grow toward the origin (in fact origin is on the other side of the abc face) { FCL_REAL mindist = -1; for(size_t i = 0; i < 3; ++i) { size_t j = nexti[i]; FCL_REAL s = vl * (d-p).dot(dl[i].cross(dl[j])); if(s > 0) // the origin is to the outside part of a triangle face, then the optimal can only be on the triangle face { ProjectResult res_triangle = projectTriangle(*vt[i], *vt[j], d, p); if(mindist < 0 || res_triangle.sqr_distance < mindist) { mindist = res_triangle.sqr_distance; res.encode = static_cast( (res_triangle.encode&1?1< 0) { const FCL_REAL t = - a.dot(d); res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l)); res.parameterization[0] = 1 - res.parameterization[1]; if(t >= l) { res.sqr_distance = b.sqrLength(); res.encode = 2; /* 0x10 */ } else if(t <= 0) { res.sqr_distance = a.sqrLength(); res.encode = 1; /* 0x01 */ } else { res.sqr_distance = (a + d * res.parameterization[1]).sqrLength(); res.encode = 3; /* 0x00 */ } } return res; } Project::ProjectResult Project::projectTriangleOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c) { ProjectResult res; static const size_t nexti[3] = {1, 2, 0}; const Vec3f* vt[] = {&a, &b, &c}; const Vec3f dl[] = {a - b, b - c, c - a}; const Vec3f& n = dl[0].cross(dl[1]); const FCL_REAL l = n.sqrLength(); if(l > 0) { FCL_REAL mindist = -1; for(size_t i = 0; i < 3; ++i) { if(vt[i]->dot(dl[i].cross(n)) > 0) // origin is to the outside part of the triangle edge, then the optimal can only be on the edge { size_t j = nexti[i]; ProjectResult res_line = projectLineOrigin(*vt[i], *vt[j]); if(mindist < 0 || res_line.sqr_distance < mindist) { mindist = res_line.sqr_distance; res.encode = static_cast(((res_line.encode&1)?1< 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng is false, then the last vertex in the tetrahedron does not grow toward the origin (in fact origin is on the other side of the abc face) { FCL_REAL mindist = -1; for(size_t i = 0; i < 3; ++i) { size_t j = nexti[i]; FCL_REAL s = vl * d.dot(dl[i].cross(dl[j])); if(s > 0) // the origin is to the outside part of a triangle face, then the optimal can only be on the triangle face { ProjectResult res_triangle = projectTriangleOrigin(*vt[i], *vt[j], d); if(mindist < 0 || res_triangle.sqr_distance < mindist) { mindist = res_triangle.sqr_distance; res.encode = static_cast( (res_triangle.encode&1?1< #include namespace fcl { /// The seed the user asked for (cannot be 0) static std::uint_fast32_t userSetSeed = 0; /// Flag indicating whether the first seed has already been generated or not static bool firstSeedGenerated = false; /// The value of the first seed static std::uint_fast32_t firstSeedValue = 0; /// Compute the first seed to be used; this function should be called only once static std::uint_fast32_t firstSeed() { static std::mutex fsLock; std::unique_lock slock(fsLock); if(firstSeedGenerated) return firstSeedValue; if(userSetSeed != 0) firstSeedValue = userSetSeed; else firstSeedValue = (std::uint_fast32_t)std::chrono::duration_cast( std::chrono::system_clock::now() - std::chrono::system_clock::time_point()).count(); firstSeedGenerated = true; return firstSeedValue; } /// We use a different random number generator for the seeds of the /// Other random generators. The root seed is from the number of /// nano-seconds in the current time. static std::uint_fast32_t nextSeed() { static std::mutex rngMutex; std::unique_lock slock(rngMutex); static std::ranlux24_base sGen; static std::uniform_int_distribution<> sDist(1, 1000000000); return sDist(sGen); } std::uint_fast32_t RNG::getSeed() { return firstSeed(); } void RNG::setSeed(std::uint_fast32_t seed) { if(firstSeedGenerated) { std::cerr << "Random number generation already started. Changing seed now will not lead to deterministic sampling." << std::endl; } if(seed == 0) { std::cerr << "Random generator seed cannot be 0. Using 1 instead." << std::endl; userSetSeed = 1; } else userSetSeed = seed; } RNG::RNG() : generator_(nextSeed()), uniDist_(0, 1), normalDist_(0, 1) { } double RNG::halfNormalReal(double r_min, double r_max, double focus) { assert(r_min <= r_max); const double mean = r_max - r_min; double v = gaussian(mean, mean / focus); if(v > mean) v = 2.0 * mean - v; double r = v >= 0.0 ? v + r_min : r_min; return r > r_max ? r_max : r; } int RNG::halfNormalInt(int r_min, int r_max, double focus) { int r = (int)floor(halfNormalReal((double)r_min, (double)(r_max) + 1.0, focus)); return (r > r_max) ? r_max : r; } // From: "Uniform Random Rotations", Ken Shoemake, Graphics Gems III, // pg. 124-132 void RNG::quaternion(double value[4]) { double x0 = uniDist_(generator_); double r1 = sqrt(1.0 - x0), r2 = sqrt(x0); double t1 = 2.0 * constants::pi * uniDist_(generator_), t2 = 2.0 * constants::pi * uniDist_(generator_); double c1 = cos(t1), s1 = sin(t1); double c2 = cos(t2), s2 = sin(t2); value[0] = s1 * r1; value[1] = c1 * r1; value[2] = s2 * r2; value[3] = c2 * r2; } // From Effective Sampling and Distance Metrics for 3D Rigid Body Path Planning, by James Kuffner, ICRA 2004 void RNG::eulerRPY(double value[3]) { value[0] = constants::pi * (2.0 * uniDist_(generator_) - 1.0); value[1] = acos(1.0 - 2.0 * uniDist_(generator_)) - constants::pi / 2.0; value[2] = constants::pi * (2.0 * uniDist_(generator_) - 1.0); } void RNG::disk(double r_min, double r_max, double& x, double& y) { double a = uniform01(); double b = uniform01(); double r = std::sqrt(a * r_max * r_max + (1 - a) * r_min * r_min); double theta = 2 * constants::pi * b; x = r * std::cos(theta); y = r * std::sin(theta); } void RNG::ball(double r_min, double r_max, double& x, double& y, double& z) { double a = uniform01(); double b = uniform01(); double c = uniform01(); double r = std::pow(a * r_max * r_max * r_max + (1 - a) * r_min * r_min * r_min, 1 / 3.0); double theta = std::acos(1 - 2 * b); double phi = 2 * constants::pi * c; double costheta = std::cos(theta); double sintheta = std::sin(theta); double cosphi = std::cos(phi); double sinphi = std::sin(phi); x = r * costheta; y = r * sintheta * cosphi; z = r * sintheta * sinphi; } } fcl-0.5.0/src/math/transform.cpp000066400000000000000000000310141274356571000165300ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/math/constants.h" #include "fcl/math/transform.h" #include namespace fcl { void Quaternion3f::fromRotation(const Matrix3f& R) { const int next[3] = {1, 2, 0}; FCL_REAL trace = R(0, 0) + R(1, 1) + R(2, 2); FCL_REAL root; if(trace > 0.0) { // |w| > 1/2, may as well choose w > 1/2 root = sqrt(trace + 1.0); // 2w data[0] = 0.5 * root; root = 0.5 / root; // 1/(4w) data[1] = (R(2, 1) - R(1, 2))*root; data[2] = (R(0, 2) - R(2, 0))*root; data[3] = (R(1, 0) - R(0, 1))*root; } else { // |w| <= 1/2 int i = 0; if(R(1, 1) > R(0, 0)) { i = 1; } if(R(2, 2) > R(i, i)) { i = 2; } int j = next[i]; int k = next[j]; root = sqrt(R(i, i) - R(j, j) - R(k, k) + 1.0); FCL_REAL* quat[3] = { &data[1], &data[2], &data[3] }; *quat[i] = 0.5 * root; root = 0.5 / root; data[0] = (R(k, j) - R(j, k)) * root; *quat[j] = (R(j, i) + R(i, j)) * root; *quat[k] = (R(k, i) + R(i, k)) * root; } } void Quaternion3f::toRotation(Matrix3f& R) const { assert (.99 < data [0]*data [0] + data [1]*data [1] + data [2]*data [2] + data [3]*data [3]); assert (data [0]*data [0] + data [1]*data [1] + data [2]*data [2] + data [3]*data [3] < 1.01); FCL_REAL twoX = 2.0*data[1]; FCL_REAL twoY = 2.0*data[2]; FCL_REAL twoZ = 2.0*data[3]; FCL_REAL twoWX = twoX*data[0]; FCL_REAL twoWY = twoY*data[0]; FCL_REAL twoWZ = twoZ*data[0]; FCL_REAL twoXX = twoX*data[1]; FCL_REAL twoXY = twoY*data[1]; FCL_REAL twoXZ = twoZ*data[1]; FCL_REAL twoYY = twoY*data[2]; FCL_REAL twoYZ = twoZ*data[2]; FCL_REAL twoZZ = twoZ*data[3]; R.setValue(1.0 - (twoYY + twoZZ), twoXY - twoWZ, twoXZ + twoWY, twoXY + twoWZ, 1.0 - (twoXX + twoZZ), twoYZ - twoWX, twoXZ - twoWY, twoYZ + twoWX, 1.0 - (twoXX + twoYY)); } void Quaternion3f::fromAxes(const Vec3f axis[3]) { // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes // article "Quaternion Calculus and Fast Animation". const int next[3] = {1, 2, 0}; FCL_REAL trace = axis[0][0] + axis[1][1] + axis[2][2]; FCL_REAL root; if(trace > 0.0) { // |w| > 1/2, may as well choose w > 1/2 root = sqrt(trace + 1.0); // 2w data[0] = 0.5 * root; root = 0.5 / root; // 1/(4w) data[1] = (axis[1][2] - axis[2][1])*root; data[2] = (axis[2][0] - axis[0][2])*root; data[3] = (axis[0][1] - axis[1][0])*root; } else { // |w| <= 1/2 int i = 0; if(axis[1][1] > axis[0][0]) { i = 1; } if(axis[2][2] > axis[i][i]) { i = 2; } int j = next[i]; int k = next[j]; root = sqrt(axis[i][i] - axis[j][j] - axis[k][k] + 1.0); FCL_REAL* quat[3] = { &data[1], &data[2], &data[3] }; *quat[i] = 0.5 * root; root = 0.5 / root; data[0] = (axis[j][k] - axis[k][j]) * root; *quat[j] = (axis[i][j] + axis[j][i]) * root; *quat[k] = (axis[i][k] + axis[k][i]) * root; } } void Quaternion3f::toAxes(Vec3f axis[3]) const { FCL_REAL twoX = 2.0*data[1]; FCL_REAL twoY = 2.0*data[2]; FCL_REAL twoZ = 2.0*data[3]; FCL_REAL twoWX = twoX*data[0]; FCL_REAL twoWY = twoY*data[0]; FCL_REAL twoWZ = twoZ*data[0]; FCL_REAL twoXX = twoX*data[1]; FCL_REAL twoXY = twoY*data[1]; FCL_REAL twoXZ = twoZ*data[1]; FCL_REAL twoYY = twoY*data[2]; FCL_REAL twoYZ = twoZ*data[2]; FCL_REAL twoZZ = twoZ*data[3]; axis[0].setValue(1.0 - (twoYY + twoZZ), twoXY + twoWZ, twoXZ - twoWY); axis[1].setValue(twoXY - twoWZ, 1.0 - (twoXX + twoZZ), twoYZ + twoWX); axis[2].setValue(twoXZ + twoWY, twoYZ - twoWX, 1.0 - (twoXX + twoYY)); } void Quaternion3f::fromAxisAngle(const Vec3f& axis, FCL_REAL angle) { FCL_REAL half_angle = 0.5 * angle; FCL_REAL sn = sin((double)half_angle); data[0] = cos((double)half_angle); data[1] = sn * axis[0]; data[2] = sn * axis[1]; data[3] = sn * axis[2]; } void Quaternion3f::toAxisAngle(Vec3f& axis, FCL_REAL& angle) const { double sqr_length = data[1] * data[1] + data[2] * data[2] + data[3] * data[3]; if(sqr_length > 0) { angle = 2.0 * acos((double)data[0]); double inv_length = 1.0 / sqrt(sqr_length); axis[0] = inv_length * data[1]; axis[1] = inv_length * data[2]; axis[2] = inv_length * data[3]; } else { angle = 0; axis[0] = 1; axis[1] = 0; axis[2] = 0; } } FCL_REAL Quaternion3f::dot(const Quaternion3f& other) const { return data[0] * other.data[0] + data[1] * other.data[1] + data[2] * other.data[2] + data[3] * other.data[3]; } Quaternion3f Quaternion3f::operator + (const Quaternion3f& other) const { return Quaternion3f(data[0] + other.data[0], data[1] + other.data[1], data[2] + other.data[2], data[3] + other.data[3]); } const Quaternion3f& Quaternion3f::operator += (const Quaternion3f& other) { data[0] += other.data[0]; data[1] += other.data[1]; data[2] += other.data[2]; data[3] += other.data[3]; return *this; } Quaternion3f Quaternion3f::operator - (const Quaternion3f& other) const { return Quaternion3f(data[0] - other.data[0], data[1] - other.data[1], data[2] - other.data[2], data[3] - other.data[3]); } const Quaternion3f& Quaternion3f::operator -= (const Quaternion3f& other) { data[0] -= other.data[0]; data[1] -= other.data[1]; data[2] -= other.data[2]; data[3] -= other.data[3]; return *this; } Quaternion3f Quaternion3f::operator * (const Quaternion3f& other) const { return Quaternion3f(data[0] * other.data[0] - data[1] * other.data[1] - data[2] * other.data[2] - data[3] * other.data[3], data[0] * other.data[1] + data[1] * other.data[0] + data[2] * other.data[3] - data[3] * other.data[2], data[0] * other.data[2] - data[1] * other.data[3] + data[2] * other.data[0] + data[3] * other.data[1], data[0] * other.data[3] + data[1] * other.data[2] - data[2] * other.data[1] + data[3] * other.data[0]); } const Quaternion3f& Quaternion3f::operator *= (const Quaternion3f& other) { FCL_REAL a = data[0] * other.data[0] - data[1] * other.data[1] - data[2] * other.data[2] - data[3] * other.data[3]; FCL_REAL b = data[0] * other.data[1] + data[1] * other.data[0] + data[2] * other.data[3] - data[3] * other.data[2]; FCL_REAL c = data[0] * other.data[2] - data[1] * other.data[3] + data[2] * other.data[0] + data[3] * other.data[1]; FCL_REAL d = data[0] * other.data[3] + data[1] * other.data[2] - data[2] * other.data[1] + data[3] * other.data[0]; data[0] = a; data[1] = b; data[2] = c; data[3] = d; return *this; } Quaternion3f Quaternion3f::operator - () const { return Quaternion3f(-data[0], -data[1], -data[2], -data[3]); } Quaternion3f Quaternion3f::operator * (FCL_REAL t) const { return Quaternion3f(data[0] * t, data[1] * t, data[2] * t, data[3] * t); } const Quaternion3f& Quaternion3f::operator *= (FCL_REAL t) { data[0] *= t; data[1] *= t; data[2] *= t; data[3] *= t; return *this; } Quaternion3f& Quaternion3f::conj() { data[1] = -data[1]; data[2] = -data[2]; data[3] = -data[3]; return *this; } Quaternion3f& Quaternion3f::inverse() { FCL_REAL sqr_length = data[0] * data[0] + data[1] * data[1] + data[2] * data[2] + data[3] * data[3]; if(sqr_length > 0) { FCL_REAL inv_length = 1 / std::sqrt(sqr_length); data[0] *= inv_length; data[1] *= (-inv_length); data[2] *= (-inv_length); data[3] *= (-inv_length); } else { data[1] = -data[1]; data[2] = -data[2]; data[3] = -data[3]; } return *this; } Vec3f Quaternion3f::transform(const Vec3f& v) const { Vec3f u(getX(), getY(), getZ()); double s = getW(); Vec3f vprime = 2*u.dot(v)*u + (s*s - u.dot(u))*v + 2*s*u.cross(v); return vprime; } Quaternion3f conj(const Quaternion3f& q) { Quaternion3f r(q); return r.conj(); } Quaternion3f inverse(const Quaternion3f& q) { Quaternion3f res(q); return res.inverse(); } void Quaternion3f::fromEuler(FCL_REAL a, FCL_REAL b, FCL_REAL c) { Matrix3f R; R.setEulerYPR(a, b, c); fromRotation(R); } void Quaternion3f::toEuler(FCL_REAL& a, FCL_REAL& b, FCL_REAL& c) const { Matrix3f R; toRotation(R); a = atan2(R(1, 0), R(0, 0)); b = asin(-R(2, 0)); c = atan2(R(2, 1), R(2, 2)); if(b == constants::pi * 0.5) { if(a > 0) a -= constants::pi; else a += constants::pi; if(c > 0) c -= constants::pi; else c += constants::pi; } } Vec3f Quaternion3f::getColumn(std::size_t i) const { switch(i) { case 0: return Vec3f(data[0] * data[0] + data[1] * data[1] - data[2] * data[2] - data[3] * data[3], 2 * (- data[0] * data[3] + data[1] * data[2]), 2 * (data[1] * data[3] + data[0] * data[2])); case 1: return Vec3f(2 * (data[1] * data[2] + data[0] * data[3]), data[0] * data[0] - data[1] * data[1] + data[2] * data[2] - data[3] * data[3], 2 * (data[2] * data[3] - data[0] * data[1])); case 2: return Vec3f(2 * (data[1] * data[3] - data[0] * data[2]), 2 * (data[2] * data[3] + data[0] * data[1]), data[0] * data[0] - data[1] * data[1] - data[2] * data[2] + data[3] * data[3]); default: return Vec3f(); } } Vec3f Quaternion3f::getRow(std::size_t i) const { switch(i) { case 0: return Vec3f(data[0] * data[0] + data[1] * data[1] - data[2] * data[2] - data[3] * data[3], 2 * (data[0] * data[3] + data[1] * data[2]), 2 * (data[1] * data[3] - data[0] * data[2])); case 1: return Vec3f(2 * (data[1] * data[2] - data[0] * data[3]), data[0] * data[0] - data[1] * data[1] + data[2] * data[2] - data[3] * data[3], 2 * (data[2] * data[3] + data[0] * data[1])); case 2: return Vec3f(2 * (data[1] * data[3] + data[0] * data[2]), 2 * (data[2] * data[3] - data[0] * data[1]), data[0] * data[0] - data[1] * data[1] - data[2] * data[2] + data[3] * data[3]); default: return Vec3f(); } } const Matrix3f& Transform3f::getRotationInternal() const { std::unique_lock slock(const_cast(lock_)); if(!matrix_set) { q.toRotation(R); matrix_set = true; } return R; } Transform3f inverse(const Transform3f& tf) { Transform3f res(tf); return res.inverse(); } void relativeTransform(const Transform3f& tf1, const Transform3f& tf2, Transform3f& tf) { const Quaternion3f& q1_inv = fcl::conj(tf1.getQuatRotation()); tf = Transform3f(q1_inv * tf2.getQuatRotation(), q1_inv.transform(tf2.getTranslation() - tf1.getTranslation())); } void relativeTransform2(const Transform3f& tf1, const Transform3f& tf2, Transform3f& tf) { const Quaternion3f& q1inv = fcl::conj(tf1.getQuatRotation()); const Quaternion3f& q2_q1inv = tf2.getQuatRotation() * q1inv; tf = Transform3f(q2_q1inv, tf2.getTranslation() - q2_q1inv.transform(tf1.getTranslation())); } } fcl-0.5.0/src/narrowphase/000077500000000000000000000000001274356571000154125ustar00rootroot00000000000000fcl-0.5.0/src/narrowphase/gjk.cpp000066400000000000000000000445221274356571000167000ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/narrowphase/gjk.h" #include "fcl/intersect.h" namespace fcl { namespace details { Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir) { switch(shape->getNodeType()) { case GEOM_TRIANGLE: { const TriangleP* triangle = static_cast(shape); FCL_REAL dota = dir.dot(triangle->a); FCL_REAL dotb = dir.dot(triangle->b); FCL_REAL dotc = dir.dot(triangle->c); if(dota > dotb) { if(dotc > dota) return triangle->c; else return triangle->a; } else { if(dotc > dotb) return triangle->c; else return triangle->b; } } break; case GEOM_BOX: { const Box* box = static_cast(shape); return Vec3f((dir[0]>0)?(box->side[0]/2):(-box->side[0]/2), (dir[1]>0)?(box->side[1]/2):(-box->side[1]/2), (dir[2]>0)?(box->side[2]/2):(-box->side[2]/2)); } break; case GEOM_SPHERE: { const Sphere* sphere = static_cast(shape); return dir * sphere->radius; } break; case GEOM_ELLIPSOID: { const Ellipsoid* ellipsoid = static_cast(shape); const FCL_REAL a2 = ellipsoid->radii[0] * ellipsoid->radii[0]; const FCL_REAL b2 = ellipsoid->radii[1] * ellipsoid->radii[1]; const FCL_REAL c2 = ellipsoid->radii[2] * ellipsoid->radii[2]; const Vec3f v(a2 * dir[0], b2 * dir[1], c2 * dir[2]); const FCL_REAL d = std::sqrt(v.dot(dir)); return v / d; } break; case GEOM_CAPSULE: { const Capsule* capsule = static_cast(shape); FCL_REAL half_h = capsule->lz * 0.5; Vec3f pos1(0, 0, half_h); Vec3f pos2(0, 0, -half_h); Vec3f v = dir * capsule->radius; pos1 += v; pos2 += v; if(dir.dot(pos1) > dir.dot(pos2)) return pos1; else return pos2; } break; case GEOM_CONE: { const Cone* cone = static_cast(shape); FCL_REAL zdist = dir[0] * dir[0] + dir[1] * dir[1]; FCL_REAL len = zdist + dir[2] * dir[2]; zdist = std::sqrt(zdist); len = std::sqrt(len); FCL_REAL half_h = cone->lz * 0.5; FCL_REAL radius = cone->radius; FCL_REAL sin_a = radius / std::sqrt(radius * radius + 4 * half_h * half_h); if(dir[2] > len * sin_a) return Vec3f(0, 0, half_h); else if(zdist > 0) { FCL_REAL rad = radius / zdist; return Vec3f(rad * dir[0], rad * dir[1], -half_h); } else return Vec3f(0, 0, -half_h); } break; case GEOM_CYLINDER: { const Cylinder* cylinder = static_cast(shape); FCL_REAL zdist = std::sqrt(dir[0] * dir[0] + dir[1] * dir[1]); FCL_REAL half_h = cylinder->lz * 0.5; if(zdist == 0.0) { return Vec3f(0, 0, (dir[2]>0)? half_h:-half_h); } else { FCL_REAL d = cylinder->radius / zdist; return Vec3f(d * dir[0], d * dir[1], (dir[2]>0)?half_h:-half_h); } } break; case GEOM_CONVEX: { const Convex* convex = static_cast(shape); FCL_REAL maxdot = - std::numeric_limits::max(); Vec3f* curp = convex->points; Vec3f bestv; for(int i = 0; i < convex->num_points; ++i, curp+=1) { FCL_REAL dot = dir.dot(*curp); if(dot > maxdot) { bestv = *curp; maxdot = dot; } } return bestv; } break; case GEOM_PLANE: break; default: ; // nothing } return Vec3f(0, 0, 0); } void GJK::initialize() { ray = Vec3f(); nfree = 0; status = Failed; current = 0; distance = 0.0; simplex = NULL; } Vec3f GJK::getGuessFromSimplex() const { return ray; } GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess) { size_t iterations = 0; FCL_REAL alpha = 0; Vec3f lastw[4]; size_t clastw = 0; free_v[0] = &store_v[0]; free_v[1] = &store_v[1]; free_v[2] = &store_v[2]; free_v[3] = &store_v[3]; nfree = 4; current = 0; status = Valid; shape = shape_; distance = 0.0; simplices[0].rank = 0; ray = guess; appendVertex(simplices[0], (ray.sqrLength() > 0) ? -ray : Vec3f(1, 0, 0)); simplices[0].p[0] = 1; ray = simplices[0].c[0]->w; lastw[0] = lastw[1] = lastw[2] = lastw[3] = ray; // cache previous support points, the new support point will compare with it to avoid too close support points do { size_t next = 1 - current; Simplex& curr_simplex = simplices[current]; Simplex& next_simplex = simplices[next]; // check A: when origin is near the existing simplex, stop FCL_REAL rl = ray.length(); if(rl < tolerance) // mean origin is near the face of original simplex, return touch { status = Inside; break; } appendVertex(curr_simplex, -ray); // see below, ray points away from origin // check B: when the new support point is close to previous support points, stop (as the new simplex is degenerated) Vec3f& w = curr_simplex.c[curr_simplex.rank - 1]->w; bool found = false; for(size_t i = 0; i < 4; ++i) { if((w - lastw[i]).sqrLength() < tolerance) { found = true; break; } } if(found) { removeVertex(simplices[current]); break; } else { lastw[clastw = (clastw+1)&3] = w; } // check C: when the new support point is close to the sub-simplex where the ray point lies, stop (as the new simplex again is degenerated) FCL_REAL omega = ray.dot(w) / rl; alpha = std::max(alpha, omega); if((rl - alpha) - tolerance * rl <= 0) { removeVertex(simplices[current]); break; } Project::ProjectResult project_res; switch(curr_simplex.rank) { case 2: project_res = Project::projectLineOrigin(curr_simplex.c[0]->w, curr_simplex.c[1]->w); break; case 3: project_res = Project::projectTriangleOrigin(curr_simplex.c[0]->w, curr_simplex.c[1]->w, curr_simplex.c[2]->w); break; case 4: project_res = Project::projectTetrahedraOrigin(curr_simplex.c[0]->w, curr_simplex.c[1]->w, curr_simplex.c[2]->w, curr_simplex.c[3]->w); break; } if(project_res.sqr_distance >= 0) { next_simplex.rank = 0; ray = Vec3f(); current = next; for(size_t i = 0; i < curr_simplex.rank; ++i) { if(project_res.encode & (1 << i)) { next_simplex.c[next_simplex.rank] = curr_simplex.c[i]; next_simplex.p[next_simplex.rank++] = project_res.parameterization[i]; // weights[i]; ray += curr_simplex.c[i]->w * project_res.parameterization[i]; // weights[i]; } else free_v[nfree++] = curr_simplex.c[i]; } if(project_res.encode == 15) status = Inside; // the origin is within the 4-simplex, collision } else { removeVertex(simplices[current]); break; } status = ((++iterations) < max_iterations) ? status : Failed; } while(status == Valid); simplex = &simplices[current]; switch(status) { case Valid: distance = ray.length(); break; case Inside: distance = 0; break; default: break; } return status; } void GJK::getSupport(const Vec3f& d, SimplexV& sv) const { sv.d = normalize(d); sv.w = shape.support(sv.d); } void GJK::getSupport(const Vec3f& d, const Vec3f& v, SimplexV& sv) const { sv.d = normalize(d); sv.w = shape.support(sv.d, v); } void GJK::removeVertex(Simplex& simplex) { free_v[nfree++] = simplex.c[--simplex.rank]; } void GJK::appendVertex(Simplex& simplex, const Vec3f& v) { simplex.p[simplex.rank] = 0; // initial weight 0 simplex.c[simplex.rank] = free_v[--nfree]; // set the memory getSupport(v, *simplex.c[simplex.rank++]); } bool GJK::encloseOrigin() { switch(simplex->rank) { case 1: { for(size_t i = 0; i < 3; ++i) { Vec3f axis; axis[i] = 1; appendVertex(*simplex, axis); if(encloseOrigin()) return true; removeVertex(*simplex); appendVertex(*simplex, -axis); if(encloseOrigin()) return true; removeVertex(*simplex); } } break; case 2: { Vec3f d = simplex->c[1]->w - simplex->c[0]->w; for(size_t i = 0; i < 3; ++i) { Vec3f axis; axis[i] = 1; Vec3f p = d.cross(axis); if(p.sqrLength() > 0) { appendVertex(*simplex, p); if(encloseOrigin()) return true; removeVertex(*simplex); appendVertex(*simplex, -p); if(encloseOrigin()) return true; removeVertex(*simplex); } } } break; case 3: { Vec3f n = (simplex->c[1]->w - simplex->c[0]->w).cross(simplex->c[2]->w - simplex->c[0]->w); if(n.sqrLength() > 0) { appendVertex(*simplex, n); if(encloseOrigin()) return true; removeVertex(*simplex); appendVertex(*simplex, -n); if(encloseOrigin()) return true; removeVertex(*simplex); } } break; case 4: { if(std::abs(triple(simplex->c[0]->w - simplex->c[3]->w, simplex->c[1]->w - simplex->c[3]->w, simplex->c[2]->w - simplex->c[3]->w)) > 0) return true; } break; } return false; } void EPA::initialize() { sv_store = new SimplexV[max_vertex_num]; fc_store = new SimplexF[max_face_num]; status = Failed; normal = Vec3f(0, 0, 0); depth = 0; nextsv = 0; for(size_t i = 0; i < max_face_num; ++i) stock.append(&fc_store[max_face_num-i-1]); } bool EPA::getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, FCL_REAL& dist) { Vec3f ba = b->w - a->w; Vec3f n_ab = ba.cross(face->n); FCL_REAL a_dot_nab = a->w.dot(n_ab); if(a_dot_nab < 0) // the origin is on the outside part of ab { // following is similar to projectOrigin for two points // however, as we dont need to compute the parameterization, dont need to compute 0 or 1 FCL_REAL a_dot_ba = a->w.dot(ba); FCL_REAL b_dot_ba = b->w.dot(ba); if(a_dot_ba > 0) dist = a->w.length(); else if(b_dot_ba < 0) dist = b->w.length(); else { FCL_REAL a_dot_b = a->w.dot(b->w); dist = std::sqrt(std::max(a->w.sqrLength() * b->w.sqrLength() - a_dot_b * a_dot_b, (FCL_REAL)0)); } return true; } return false; } EPA::SimplexF* EPA::newFace(SimplexV* a, SimplexV* b, SimplexV* c, bool forced) { if(stock.root) { SimplexF* face = stock.root; stock.remove(face); hull.append(face); face->pass = 0; face->c[0] = a; face->c[1] = b; face->c[2] = c; face->n = (b->w - a->w).cross(c->w - a->w); FCL_REAL l = face->n.length(); if(l > tolerance) { if(!(getEdgeDist(face, a, b, face->d) || getEdgeDist(face, b, c, face->d) || getEdgeDist(face, c, a, face->d))) { face->d = a->w.dot(face->n) / l; } face->n /= l; if(forced || face->d >= -tolerance) return face; else status = NonConvex; } else status = Degenerated; hull.remove(face); stock.append(face); return NULL; } status = stock.root ? OutOfVertices : OutOfFaces; return NULL; } /** \brief Find the best polytope face to split */ EPA::SimplexF* EPA::findBest() { SimplexF* minf = hull.root; FCL_REAL mind = minf->d * minf->d; for(SimplexF* f = minf->l[1]; f; f = f->l[1]) { FCL_REAL sqd = f->d * f->d; if(sqd < mind) { minf = f; mind = sqd; } } return minf; } EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) { GJK::Simplex& simplex = *gjk.getSimplex(); if((simplex.rank > 1) && gjk.encloseOrigin()) { while(hull.root) { SimplexF* f = hull.root; hull.remove(f); stock.append(f); } status = Valid; nextsv = 0; if((simplex.c[0]->w - simplex.c[3]->w).dot((simplex.c[1]->w - simplex.c[3]->w).cross(simplex.c[2]->w - simplex.c[3]->w)) < 0) { SimplexV* tmp = simplex.c[0]; simplex.c[0] = simplex.c[1]; simplex.c[1] = tmp; FCL_REAL tmpv = simplex.p[0]; simplex.p[0] = simplex.p[1]; simplex.p[1] = tmpv; } SimplexF* tetrahedron[] = {newFace(simplex.c[0], simplex.c[1], simplex.c[2], true), newFace(simplex.c[1], simplex.c[0], simplex.c[3], true), newFace(simplex.c[2], simplex.c[1], simplex.c[3], true), newFace(simplex.c[0], simplex.c[2], simplex.c[3], true) }; if(hull.count == 4) { SimplexF* best = findBest(); // find the best face (the face with the minimum distance to origin) to split SimplexF outer = *best; size_t pass = 0; size_t iterations = 0; // set the face connectivity bind(tetrahedron[0], 0, tetrahedron[1], 0); bind(tetrahedron[0], 1, tetrahedron[2], 0); bind(tetrahedron[0], 2, tetrahedron[3], 0); bind(tetrahedron[1], 1, tetrahedron[3], 2); bind(tetrahedron[1], 2, tetrahedron[2], 1); bind(tetrahedron[2], 2, tetrahedron[3], 1); status = Valid; for(; iterations < max_iterations; ++iterations) { if(nextsv < max_vertex_num) { SimplexHorizon horizon; SimplexV* w = &sv_store[nextsv++]; bool valid = true; best->pass = ++pass; gjk.getSupport(best->n, *w); FCL_REAL wdist = best->n.dot(w->w) - best->d; if(wdist > tolerance) { for(size_t j = 0; (j < 3) && valid; ++j) { valid &= expand(pass, w, best->f[j], best->e[j], horizon); } if(valid && horizon.nf >= 3) { // need to add the edge connectivity between first and last faces bind(horizon.ff, 2, horizon.cf, 1); hull.remove(best); stock.append(best); best = findBest(); outer = *best; } else { status = InvalidHull; break; } } else { status = AccuracyReached; break; } } else { status = OutOfVertices; break; } } Vec3f projection = outer.n * outer.d; normal = outer.n; depth = outer.d; result.rank = 3; result.c[0] = outer.c[0]; result.c[1] = outer.c[1]; result.c[2] = outer.c[2]; result.p[0] = ((outer.c[1]->w - projection).cross(outer.c[2]->w - projection)).length(); result.p[1] = ((outer.c[2]->w - projection).cross(outer.c[0]->w - projection)).length(); result.p[2] = ((outer.c[0]->w - projection).cross(outer.c[1]->w - projection)).length(); FCL_REAL sum = result.p[0] + result.p[1] + result.p[2]; result.p[0] /= sum; result.p[1] /= sum; result.p[2] /= sum; return status; } } status = FallBack; normal = -guess; FCL_REAL nl = normal.length(); if(nl > 0) normal /= nl; else normal = Vec3f(1, 0, 0); depth = 0; result.rank = 1; result.c[0] = simplex.c[0]; result.p[0] = 1; return status; } /** \brief the goal is to add a face connecting vertex w and face edge f[e] */ bool EPA::expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon& horizon) { static const size_t nexti[] = {1, 2, 0}; static const size_t previ[] = {2, 0, 1}; if(f->pass != pass) { const size_t e1 = nexti[e]; // case 1: the new face is not degenerated, i.e., the new face is not coplanar with the old face f. if(f->n.dot(w->w) - f->d < -tolerance) { SimplexF* nf = newFace(f->c[e1], f->c[e], w, false); if(nf) { // add face-face connectivity bind(nf, 0, f, e); // if there is last face in the horizon, then need to add another connectivity, i.e. the edge connecting the current new add edge and the last new add edge. // This does not finish all the connectivities because the final face need to connect with the first face, this will be handled in the evaluate function. // Notice the face is anti-clockwise, so the edges are 0 (bottom), 1 (right), 2 (left) if(horizon.cf) bind(nf, 2, horizon.cf, 1); else horizon.ff = nf; horizon.cf = nf; ++horizon.nf; return true; } } else // case 2: the new face is coplanar with the old face f. We need to add two faces and delete the old face { const size_t e2 = previ[e]; f->pass = pass; if(expand(pass, w, f->f[e1], f->e[e1], horizon) && expand(pass, w, f->f[e2], f->e[e2], horizon)) { hull.remove(f); stock.append(f); return true; } } } return false; } } // details } // fcl fcl-0.5.0/src/narrowphase/gjk_libccd.cpp000066400000000000000000000701651274356571000202020ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/narrowphase/gjk_libccd.h" #include "fcl/ccd/simplex.h" #include #include namespace fcl { namespace details { struct ccd_obj_t { ccd_vec3_t pos; ccd_quat_t rot, rot_inv; }; struct ccd_box_t : public ccd_obj_t { ccd_real_t dim[3]; }; struct ccd_cap_t : public ccd_obj_t { ccd_real_t radius, height; }; struct ccd_cyl_t : public ccd_obj_t { ccd_real_t radius, height; }; struct ccd_cone_t : public ccd_obj_t { ccd_real_t radius, height; }; struct ccd_sphere_t : public ccd_obj_t { ccd_real_t radius; }; struct ccd_ellipsoid_t : public ccd_obj_t { ccd_real_t radii[3]; }; struct ccd_convex_t : public ccd_obj_t { const Convex* convex; }; struct ccd_triangle_t : public ccd_obj_t { ccd_vec3_t p[3]; ccd_vec3_t c; }; namespace libccd_extension { static ccd_real_t simplexReduceToTriangle(ccd_simplex_t *simplex, ccd_real_t dist, ccd_vec3_t *best_witness) { ccd_real_t newdist; ccd_vec3_t witness; int best = -1; int i; // try the fourth point in all three positions for (i = 0; i < 3; i++){ newdist = ccdVec3PointTriDist2(ccd_vec3_origin, &ccdSimplexPoint(simplex, (i == 0 ? 3 : 0))->v, &ccdSimplexPoint(simplex, (i == 1 ? 3 : 1))->v, &ccdSimplexPoint(simplex, (i == 2 ? 3 : 2))->v, &witness); newdist = CCD_SQRT(newdist); // record the best triangle if (newdist < dist){ dist = newdist; best = i; ccdVec3Copy(best_witness, &witness); } } if (best >= 0){ ccdSimplexSet(simplex, best, ccdSimplexPoint(simplex, 3)); } ccdSimplexSetSize(simplex, 3); return dist; } _ccd_inline void tripleCross(const ccd_vec3_t *a, const ccd_vec3_t *b, const ccd_vec3_t *c, ccd_vec3_t *d) { ccd_vec3_t e; ccdVec3Cross(&e, a, b); ccdVec3Cross(d, &e, c); } static int doSimplex2(ccd_simplex_t *simplex, ccd_vec3_t *dir) { const ccd_support_t *A, *B; ccd_vec3_t AB, AO, tmp; ccd_real_t dot; // get last added as A A = ccdSimplexLast(simplex); // get the other point B = ccdSimplexPoint(simplex, 0); // compute AB oriented segment ccdVec3Sub2(&AB, &B->v, &A->v); // compute AO vector ccdVec3Copy(&AO, &A->v); ccdVec3Scale(&AO, -CCD_ONE); // dot product AB . AO dot = ccdVec3Dot(&AB, &AO); // check if origin doesn't lie on AB segment ccdVec3Cross(&tmp, &AB, &AO); if (ccdIsZero(ccdVec3Len2(&tmp)) && dot > CCD_ZERO){ return 1; } // check if origin is in area where AB segment is if (ccdIsZero(dot) || dot < CCD_ZERO){ // origin is in outside are of A ccdSimplexSet(simplex, 0, A); ccdSimplexSetSize(simplex, 1); ccdVec3Copy(dir, &AO); }else{ // origin is in area where AB segment is // keep simplex untouched and set direction to // AB x AO x AB tripleCross(&AB, &AO, &AB, dir); } return 0; } static int doSimplex3(ccd_simplex_t *simplex, ccd_vec3_t *dir) { const ccd_support_t *A, *B, *C; ccd_vec3_t AO, AB, AC, ABC, tmp; ccd_real_t dot, dist; // get last added as A A = ccdSimplexLast(simplex); // get the other points B = ccdSimplexPoint(simplex, 1); C = ccdSimplexPoint(simplex, 0); // check touching contact dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v, NULL); if (ccdIsZero(dist)){ return 1; } // check if triangle is really triangle (has area > 0) // if not simplex can't be expanded and thus no itersection is found if (ccdVec3Eq(&A->v, &B->v) || ccdVec3Eq(&A->v, &C->v)){ return -1; } // compute AO vector ccdVec3Copy(&AO, &A->v); ccdVec3Scale(&AO, -CCD_ONE); // compute AB and AC segments and ABC vector (perpendircular to triangle) ccdVec3Sub2(&AB, &B->v, &A->v); ccdVec3Sub2(&AC, &C->v, &A->v); ccdVec3Cross(&ABC, &AB, &AC); ccdVec3Cross(&tmp, &ABC, &AC); dot = ccdVec3Dot(&tmp, &AO); if (ccdIsZero(dot) || dot > CCD_ZERO){ dot = ccdVec3Dot(&AC, &AO); if (ccdIsZero(dot) || dot > CCD_ZERO){ // C is already in place ccdSimplexSet(simplex, 1, A); ccdSimplexSetSize(simplex, 2); tripleCross(&AC, &AO, &AC, dir); }else{ ccd_do_simplex3_45: dot = ccdVec3Dot(&AB, &AO); if (ccdIsZero(dot) || dot > CCD_ZERO){ ccdSimplexSet(simplex, 0, B); ccdSimplexSet(simplex, 1, A); ccdSimplexSetSize(simplex, 2); tripleCross(&AB, &AO, &AB, dir); }else{ ccdSimplexSet(simplex, 0, A); ccdSimplexSetSize(simplex, 1); ccdVec3Copy(dir, &AO); } } }else{ ccdVec3Cross(&tmp, &AB, &ABC); dot = ccdVec3Dot(&tmp, &AO); if (ccdIsZero(dot) || dot > CCD_ZERO){ goto ccd_do_simplex3_45; }else{ dot = ccdVec3Dot(&ABC, &AO); if (ccdIsZero(dot) || dot > CCD_ZERO){ ccdVec3Copy(dir, &ABC); }else{ ccd_support_t Ctmp; ccdSupportCopy(&Ctmp, C); ccdSimplexSet(simplex, 0, B); ccdSimplexSet(simplex, 1, &Ctmp); ccdVec3Copy(dir, &ABC); ccdVec3Scale(dir, -CCD_ONE); } } } return 0; } static int doSimplex4(ccd_simplex_t *simplex, ccd_vec3_t *dir) { const ccd_support_t *A, *B, *C, *D; ccd_vec3_t AO, AB, AC, AD, ABC, ACD, ADB; int B_on_ACD, C_on_ADB, D_on_ABC; int AB_O, AC_O, AD_O; ccd_real_t dist; // get last added as A A = ccdSimplexLast(simplex); // get the other points B = ccdSimplexPoint(simplex, 2); C = ccdSimplexPoint(simplex, 1); D = ccdSimplexPoint(simplex, 0); // check if tetrahedron is really tetrahedron (has volume > 0) // if it is not simplex can't be expanded and thus no intersection is // found dist = ccdVec3PointTriDist2(&A->v, &B->v, &C->v, &D->v, NULL); if (ccdIsZero(dist)){ return -1; } // check if origin lies on some of tetrahedron's face - if so objects // intersect dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v, NULL); if (ccdIsZero(dist)) return 1; dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &C->v, &D->v, NULL); if (ccdIsZero(dist)) return 1; dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &D->v, NULL); if (ccdIsZero(dist)) return 1; dist = ccdVec3PointTriDist2(ccd_vec3_origin, &B->v, &C->v, &D->v, NULL); if (ccdIsZero(dist)) return 1; // compute AO, AB, AC, AD segments and ABC, ACD, ADB normal vectors ccdVec3Copy(&AO, &A->v); ccdVec3Scale(&AO, -CCD_ONE); ccdVec3Sub2(&AB, &B->v, &A->v); ccdVec3Sub2(&AC, &C->v, &A->v); ccdVec3Sub2(&AD, &D->v, &A->v); ccdVec3Cross(&ABC, &AB, &AC); ccdVec3Cross(&ACD, &AC, &AD); ccdVec3Cross(&ADB, &AD, &AB); // side (positive or negative) of B, C, D relative to planes ACD, ADB // and ABC respectively B_on_ACD = ccdSign(ccdVec3Dot(&ACD, &AB)); C_on_ADB = ccdSign(ccdVec3Dot(&ADB, &AC)); D_on_ABC = ccdSign(ccdVec3Dot(&ABC, &AD)); // whether origin is on same side of ACD, ADB, ABC as B, C, D // respectively AB_O = ccdSign(ccdVec3Dot(&ACD, &AO)) == B_on_ACD; AC_O = ccdSign(ccdVec3Dot(&ADB, &AO)) == C_on_ADB; AD_O = ccdSign(ccdVec3Dot(&ABC, &AO)) == D_on_ABC; if (AB_O && AC_O && AD_O){ // origin is in tetrahedron return 1; // rearrange simplex to triangle and call doSimplex3() }else if (!AB_O){ // B is farthest from the origin among all of the tetrahedron's // points, so remove it from the list and go on with the triangle // case // D and C are in place ccdSimplexSet(simplex, 2, A); ccdSimplexSetSize(simplex, 3); }else if (!AC_O){ // C is farthest ccdSimplexSet(simplex, 1, D); ccdSimplexSet(simplex, 0, B); ccdSimplexSet(simplex, 2, A); ccdSimplexSetSize(simplex, 3); }else{ // (!AD_O) ccdSimplexSet(simplex, 0, C); ccdSimplexSet(simplex, 1, B); ccdSimplexSet(simplex, 2, A); ccdSimplexSetSize(simplex, 3); } return doSimplex3(simplex, dir); } static int doSimplex(ccd_simplex_t *simplex, ccd_vec3_t *dir) { if (ccdSimplexSize(simplex) == 2){ // simplex contains segment only one segment return doSimplex2(simplex, dir); }else if (ccdSimplexSize(simplex) == 3){ // simplex contains triangle return doSimplex3(simplex, dir); }else{ // ccdSimplexSize(simplex) == 4 // tetrahedron - this is the only shape which can encapsule origin // so doSimplex4() also contains test on it return doSimplex4(simplex, dir); } } static int __ccdGJK(const void *obj1, const void *obj2, const ccd_t *ccd, ccd_simplex_t *simplex) { unsigned long iterations; ccd_vec3_t dir; // direction vector ccd_support_t last; // last support point int do_simplex_res; // initialize simplex struct ccdSimplexInit(simplex); // get first direction ccd->first_dir(obj1, obj2, &dir); // get first support point __ccdSupport(obj1, obj2, &dir, ccd, &last); // and add this point to simplex as last one ccdSimplexAdd(simplex, &last); // set up direction vector to as (O - last) which is exactly -last ccdVec3Copy(&dir, &last.v); ccdVec3Scale(&dir, -CCD_ONE); // start iterations for (iterations = 0UL; iterations < ccd->max_iterations; ++iterations) { // obtain support point __ccdSupport(obj1, obj2, &dir, ccd, &last); // check if farthest point in Minkowski difference in direction dir // isn't somewhere before origin (the test on negative dot product) // - because if it is, objects are not intersecting at all. if (ccdVec3Dot(&last.v, &dir) < CCD_ZERO){ return -1; // intersection not found } // add last support vector to simplex ccdSimplexAdd(simplex, &last); // if doSimplex returns 1 if objects intersect, -1 if objects don't // intersect and 0 if algorithm should continue do_simplex_res = doSimplex(simplex, &dir); if (do_simplex_res == 1){ return 0; // intersection found }else if (do_simplex_res == -1){ return -1; // intersection not found } if (ccdIsZero(ccdVec3Len2(&dir))){ return -1; // intersection not found } } // intersection wasn't found return -1; } /// change the libccd distance to add two closest points ccd_real_t ccdGJKDist2(const void *obj1, const void *obj2, const ccd_t *ccd, ccd_vec3_t* p1, ccd_vec3_t* p2) { unsigned long iterations; ccd_simplex_t simplex; ccd_support_t last; // last support point ccd_vec3_t dir; // direction vector ccd_real_t dist, last_dist; // first find an intersection if (__ccdGJK(obj1, obj2, ccd, &simplex) == 0) return -CCD_ONE; last_dist = CCD_REAL_MAX; for (iterations = 0UL; iterations < ccd->max_iterations; ++iterations) { // get a next direction vector // we are trying to find out a point on the minkowski difference // that is nearest to the origin, so we obtain a point on the // simplex that is nearest and try to exapand the simplex towards // the origin if (ccdSimplexSize(&simplex) == 1){ ccdVec3Copy(&dir, &ccdSimplexPoint(&simplex, 0)->v); dist = ccdVec3Len2(&ccdSimplexPoint(&simplex, 0)->v); dist = CCD_SQRT(dist); }else if (ccdSimplexSize(&simplex) == 2){ dist = ccdVec3PointSegmentDist2(ccd_vec3_origin, &ccdSimplexPoint(&simplex, 0)->v, &ccdSimplexPoint(&simplex, 1)->v, &dir); dist = CCD_SQRT(dist); }else if(ccdSimplexSize(&simplex) == 3){ dist = ccdVec3PointTriDist2(ccd_vec3_origin, &ccdSimplexPoint(&simplex, 0)->v, &ccdSimplexPoint(&simplex, 1)->v, &ccdSimplexPoint(&simplex, 2)->v, &dir); dist = CCD_SQRT(dist); }else{ // ccdSimplexSize(&simplex) == 4 dist = simplexReduceToTriangle(&simplex, last_dist, &dir); } // touching contact -- do we really need this? // maybe __ccdGJK() solve this alredy. if (ccdIsZero(dist)) return -CCD_ONE; // check whether we improved for at least a minimum tolerance if ((last_dist - dist) < ccd->dist_tolerance) { if(p1) *p1 = last.v1; if(p2) *p2 = last.v2; return dist; } // point direction towards the origin ccdVec3Scale(&dir, -CCD_ONE); ccdVec3Normalize(&dir); // find out support point __ccdSupport(obj1, obj2, &dir, ccd, &last); // record last distance last_dist = dist; // check whether we improved for at least a minimum tolerance // this is here probably only for a degenerate cases when we got a // point that is already in the simplex dist = ccdVec3Len2(&last.v); dist = CCD_SQRT(dist); if (CCD_FABS(last_dist - dist) < ccd->dist_tolerance) { if(p1) *p1 = last.v1; if(p2) *p2 = last.v2; return last_dist; } // add a point to simplex ccdSimplexAdd(&simplex, &last); } return -CCD_REAL(1.); } } /** Basic shape to ccd shape */ static void shapeToGJK(const ShapeBase& s, const Transform3f& tf, ccd_obj_t* o) { const Quaternion3f& q = tf.getQuatRotation(); const Vec3f& T = tf.getTranslation(); ccdVec3Set(&o->pos, T[0], T[1], T[2]); ccdQuatSet(&o->rot, q.getX(), q.getY(), q.getZ(), q.getW()); ccdQuatInvert2(&o->rot_inv, &o->rot); } static void boxToGJK(const Box& s, const Transform3f& tf, ccd_box_t* box) { shapeToGJK(s, tf, box); box->dim[0] = s.side[0] / 2.0; box->dim[1] = s.side[1] / 2.0; box->dim[2] = s.side[2] / 2.0; } static void capToGJK(const Capsule& s, const Transform3f& tf, ccd_cap_t* cap) { shapeToGJK(s, tf, cap); cap->radius = s.radius; cap->height = s.lz / 2; } static void cylToGJK(const Cylinder& s, const Transform3f& tf, ccd_cyl_t* cyl) { shapeToGJK(s, tf, cyl); cyl->radius = s.radius; cyl->height = s.lz / 2; } static void coneToGJK(const Cone& s, const Transform3f& tf, ccd_cone_t* cone) { shapeToGJK(s, tf, cone); cone->radius = s.radius; cone->height = s.lz / 2; } static void sphereToGJK(const Sphere& s, const Transform3f& tf, ccd_sphere_t* sph) { shapeToGJK(s, tf, sph); sph->radius = s.radius; } static void ellipsoidToGJK(const Ellipsoid& s, const Transform3f& tf, ccd_ellipsoid_t* ellipsoid) { shapeToGJK(s, tf, ellipsoid); ellipsoid->radii[0] = s.radii[0]; ellipsoid->radii[1] = s.radii[1]; ellipsoid->radii[2] = s.radii[2]; } static void convexToGJK(const Convex& s, const Transform3f& tf, ccd_convex_t* conv) { shapeToGJK(s, tf, conv); conv->convex = &s; } /** Support functions */ static void supportBox(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v) { const ccd_box_t* o = static_cast(obj); ccd_vec3_t dir; ccdVec3Copy(&dir, dir_); ccdQuatRotVec(&dir, &o->rot_inv); ccdVec3Set(v, ccdSign(ccdVec3X(&dir)) * o->dim[0], ccdSign(ccdVec3Y(&dir)) * o->dim[1], ccdSign(ccdVec3Z(&dir)) * o->dim[2]); ccdQuatRotVec(v, &o->rot); ccdVec3Add(v, &o->pos); } static void supportCap(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v) { const ccd_cap_t* o = static_cast(obj); ccd_vec3_t dir, pos1, pos2; ccdVec3Copy(&dir, dir_); ccdQuatRotVec(&dir, &o->rot_inv); ccdVec3Set(&pos1, CCD_ZERO, CCD_ZERO, o->height); ccdVec3Set(&pos2, CCD_ZERO, CCD_ZERO, -o->height); ccdVec3Copy(v, &dir); ccdVec3Normalize (v); ccdVec3Scale(v, o->radius); ccdVec3Add(&pos1, v); ccdVec3Add(&pos2, v); if(ccdVec3Z (&dir) > 0) ccdVec3Copy(v, &pos1); else ccdVec3Copy(v, &pos2); // transform support vertex ccdQuatRotVec(v, &o->rot); ccdVec3Add(v, &o->pos); } static void supportCyl(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v) { const ccd_cyl_t* cyl = static_cast(obj); ccd_vec3_t dir; double zdist, rad; ccdVec3Copy(&dir, dir_); ccdQuatRotVec(&dir, &cyl->rot_inv); zdist = dir.v[0] * dir.v[0] + dir.v[1] * dir.v[1]; zdist = sqrt(zdist); if(ccdIsZero(zdist)) ccdVec3Set(v, 0., 0., ccdSign(ccdVec3Z(&dir)) * cyl->height); else { rad = cyl->radius / zdist; ccdVec3Set(v, rad * ccdVec3X(&dir), rad * ccdVec3Y(&dir), ccdSign(ccdVec3Z(&dir)) * cyl->height); } // transform support vertex ccdQuatRotVec(v, &cyl->rot); ccdVec3Add(v, &cyl->pos); } static void supportCone(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v) { const ccd_cone_t* cone = static_cast(obj); ccd_vec3_t dir; ccdVec3Copy(&dir, dir_); ccdQuatRotVec(&dir, &cone->rot_inv); double zdist, len, rad; zdist = dir.v[0] * dir.v[0] + dir.v[1] * dir.v[1]; len = zdist + dir.v[2] * dir.v[2]; zdist = sqrt(zdist); len = sqrt(len); double sin_a = cone->radius / sqrt(cone->radius * cone->radius + 4 * cone->height * cone->height); if(dir.v[2] > len * sin_a) ccdVec3Set(v, 0., 0., cone->height); else if(zdist > 0) { rad = cone->radius / zdist; ccdVec3Set(v, rad * ccdVec3X(&dir), rad * ccdVec3Y(&dir), -cone->height); } else ccdVec3Set(v, 0, 0, -cone->height); // transform support vertex ccdQuatRotVec(v, &cone->rot); ccdVec3Add(v, &cone->pos); } static void supportSphere(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v) { const ccd_sphere_t* s = static_cast(obj); ccd_vec3_t dir; ccdVec3Copy(&dir, dir_); ccdQuatRotVec(&dir, &s->rot_inv); ccdVec3Copy(v, &dir); ccdVec3Scale(v, s->radius); ccdVec3Scale(v, CCD_ONE / CCD_SQRT(ccdVec3Len2(&dir))); // transform support vertex ccdQuatRotVec(v, &s->rot); ccdVec3Add(v, &s->pos); } static void supportEllipsoid(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v) { const ccd_ellipsoid_t* s = static_cast(obj); ccd_vec3_t dir; ccdVec3Copy(&dir, dir_); ccdQuatRotVec(&dir, &s->rot_inv); ccd_vec3_t abc2; abc2.v[0] = s->radii[0] * s->radii[0]; abc2.v[1] = s->radii[1] * s->radii[1]; abc2.v[2] = s->radii[2] * s->radii[2]; v->v[0] = abc2.v[0] * dir.v[0]; v->v[1] = abc2.v[1] * dir.v[1]; v->v[2] = abc2.v[2] * dir.v[2]; ccdVec3Scale(v, CCD_ONE / CCD_SQRT(ccdVec3Dot(v, &dir))); // transform support vertex ccdQuatRotVec(v, &s->rot); ccdVec3Add(v, &s->pos); } static void supportConvex(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v) { const ccd_convex_t* c = (const ccd_convex_t*)obj; ccd_vec3_t dir, p; ccd_real_t maxdot, dot; int i; Vec3f* curp; const Vec3f& center = c->convex->center; ccdVec3Copy(&dir, dir_); ccdQuatRotVec(&dir, &c->rot_inv); maxdot = -CCD_REAL_MAX; curp = c->convex->points; for(i = 0; i < c->convex->num_points; ++i, curp += 1) { ccdVec3Set(&p, (*curp)[0] - center[0], (*curp)[1] - center[1], (*curp)[2] - center[2]); dot = ccdVec3Dot(&dir, &p); if(dot > maxdot) { ccdVec3Set(v, (*curp)[0], (*curp)[1], (*curp)[2]); maxdot = dot; } } // transform support vertex ccdQuatRotVec(v, &c->rot); ccdVec3Add(v, &c->pos); } static void supportTriangle(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v) { const ccd_triangle_t* tri = static_cast(obj); ccd_vec3_t dir, p; ccd_real_t maxdot, dot; int i; ccdVec3Copy(&dir, dir_); ccdQuatRotVec(&dir, &tri->rot_inv); maxdot = -CCD_REAL_MAX; for(i = 0; i < 3; ++i) { ccdVec3Set(&p, tri->p[i].v[0] - tri->c.v[0], tri->p[i].v[1] - tri->c.v[1], tri->p[i].v[2] - tri->c.v[2]); dot = ccdVec3Dot(&dir, &p); if(dot > maxdot) { ccdVec3Copy(v, &tri->p[i]); maxdot = dot; } } // transform support vertex ccdQuatRotVec(v, &tri->rot); ccdVec3Add(v, &tri->pos); } static void centerShape(const void* obj, ccd_vec3_t* c) { const ccd_obj_t *o = static_cast(obj); ccdVec3Copy(c, &o->pos); } static void centerConvex(const void* obj, ccd_vec3_t* c) { const ccd_convex_t *o = static_cast(obj); ccdVec3Set(c, o->convex->center[0], o->convex->center[1], o->convex->center[2]); ccdQuatRotVec(c, &o->rot); ccdVec3Add(c, &o->pos); } static void centerTriangle(const void* obj, ccd_vec3_t* c) { const ccd_triangle_t *o = static_cast(obj); ccdVec3Copy(c, &o->c); ccdQuatRotVec(c, &o->rot); ccdVec3Add(c, &o->pos); } bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1, void* obj2, ccd_support_fn supp2, ccd_center_fn cen2, unsigned int max_iterations, FCL_REAL tolerance, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) { ccd_t ccd; int res; ccd_real_t depth; ccd_vec3_t dir, pos; CCD_INIT(&ccd); ccd.support1 = supp1; ccd.support2 = supp2; ccd.center1 = cen1; ccd.center2 = cen2; ccd.max_iterations = max_iterations; ccd.mpr_tolerance = tolerance; if(!contact_points) { return ccdMPRIntersect(obj1, obj2, &ccd); } /// libccd returns dir and pos in world space and dir is pointing from object 1 to object 2 res = ccdMPRPenetration(obj1, obj2, &ccd, &depth, &dir, &pos); if(res == 0) { contact_points->setValue(ccdVec3X(&pos), ccdVec3Y(&pos), ccdVec3Z(&pos)); *penetration_depth = depth; normal->setValue(ccdVec3X(&dir), ccdVec3Y(&dir), ccdVec3Z(&dir)); return true; } return false; } /// p1 and p2 are in global coordinate, so needs transform in the narrowphase.h functions bool GJKDistance(void* obj1, ccd_support_fn supp1, void* obj2, ccd_support_fn supp2, unsigned int max_iterations, FCL_REAL tolerance, FCL_REAL* res, Vec3f* p1, Vec3f* p2) { ccd_t ccd; ccd_real_t dist; CCD_INIT(&ccd); ccd.support1 = supp1; ccd.support2 = supp2; ccd.max_iterations = max_iterations; ccd.dist_tolerance = tolerance; ccd_vec3_t p1_, p2_; dist = libccd_extension::ccdGJKDist2(obj1, obj2, &ccd, &p1_, &p2_); if(p1) p1->setValue(ccdVec3X(&p1_), ccdVec3Y(&p1_), ccdVec3Z(&p1_)); if(p2) p2->setValue(ccdVec3X(&p2_), ccdVec3Y(&p2_), ccdVec3Z(&p2_)); if(res) *res = dist; if(dist < 0) return false; else return true; } GJKSupportFunction GJKInitializer::getSupportFunction() { return &supportCyl; } GJKCenterFunction GJKInitializer::getCenterFunction() { return ¢erShape; } void* GJKInitializer::createGJKObject(const Cylinder& s, const Transform3f& tf) { ccd_cyl_t* o = new ccd_cyl_t; cylToGJK(s, tf, o); return o; } void GJKInitializer::deleteGJKObject(void* o_) { ccd_cyl_t* o = static_cast(o_); delete o; } GJKSupportFunction GJKInitializer::getSupportFunction() { return &supportSphere; } GJKCenterFunction GJKInitializer::getCenterFunction() { return ¢erShape; } void* GJKInitializer::createGJKObject(const Sphere& s, const Transform3f& tf) { ccd_sphere_t* o = new ccd_sphere_t; sphereToGJK(s, tf, o); return o; } void GJKInitializer::deleteGJKObject(void* o_) { ccd_sphere_t* o = static_cast(o_); delete o; } GJKSupportFunction GJKInitializer::getSupportFunction() { return &supportEllipsoid; } GJKCenterFunction GJKInitializer::getCenterFunction() { return ¢erShape; } void* GJKInitializer::createGJKObject(const Ellipsoid& s, const Transform3f& tf) { ccd_ellipsoid_t* o = new ccd_ellipsoid_t; ellipsoidToGJK(s, tf, o); return o; } void GJKInitializer::deleteGJKObject(void* o_) { ccd_ellipsoid_t* o = static_cast(o_); delete o; } GJKSupportFunction GJKInitializer::getSupportFunction() { return &supportBox; } GJKCenterFunction GJKInitializer::getCenterFunction() { return ¢erShape; } void* GJKInitializer::createGJKObject(const Box& s, const Transform3f& tf) { ccd_box_t* o = new ccd_box_t; boxToGJK(s, tf, o); return o; } void GJKInitializer::deleteGJKObject(void* o_) { ccd_box_t* o = static_cast(o_); delete o; } GJKSupportFunction GJKInitializer::getSupportFunction() { return &supportCap; } GJKCenterFunction GJKInitializer::getCenterFunction() { return ¢erShape; } void* GJKInitializer::createGJKObject(const Capsule& s, const Transform3f& tf) { ccd_cap_t* o = new ccd_cap_t; capToGJK(s, tf, o); return o; } void GJKInitializer::deleteGJKObject(void* o_) { ccd_cap_t* o = static_cast(o_); delete o; } GJKSupportFunction GJKInitializer::getSupportFunction() { return &supportCone; } GJKCenterFunction GJKInitializer::getCenterFunction() { return ¢erShape; } void* GJKInitializer::createGJKObject(const Cone& s, const Transform3f& tf) { ccd_cone_t* o = new ccd_cone_t; coneToGJK(s, tf, o); return o; } void GJKInitializer::deleteGJKObject(void* o_) { ccd_cone_t* o = static_cast(o_); delete o; } GJKSupportFunction GJKInitializer::getSupportFunction() { return &supportConvex; } GJKCenterFunction GJKInitializer::getCenterFunction() { return ¢erConvex; } void* GJKInitializer::createGJKObject(const Convex& s, const Transform3f& tf) { ccd_convex_t* o = new ccd_convex_t; convexToGJK(s, tf, o); return o; } void GJKInitializer::deleteGJKObject(void* o_) { ccd_convex_t* o = static_cast(o_); delete o; } GJKSupportFunction triGetSupportFunction() { return &supportTriangle; } GJKCenterFunction triGetCenterFunction() { return ¢erTriangle; } void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3) { ccd_triangle_t* o = new ccd_triangle_t; Vec3f center((P1[0] + P2[0] + P3[0]) / 3, (P1[1] + P2[1] + P3[1]) / 3, (P1[2] + P2[2] + P3[2]) / 3); ccdVec3Set(&o->p[0], P1[0], P1[1], P1[2]); ccdVec3Set(&o->p[1], P2[0], P2[1], P2[2]); ccdVec3Set(&o->p[2], P3[0], P3[1], P3[2]); ccdVec3Set(&o->c, center[0], center[1], center[2]); ccdVec3Set(&o->pos, 0., 0., 0.); ccdQuatSet(&o->rot, 0., 0., 0., 1.); ccdQuatInvert2(&o->rot_inv, &o->rot); return o; } void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf) { ccd_triangle_t* o = new ccd_triangle_t; Vec3f center((P1[0] + P2[0] + P3[0]) / 3, (P1[1] + P2[1] + P3[1]) / 3, (P1[2] + P2[2] + P3[2]) / 3); ccdVec3Set(&o->p[0], P1[0], P1[1], P1[2]); ccdVec3Set(&o->p[1], P2[0], P2[1], P2[2]); ccdVec3Set(&o->p[2], P3[0], P3[1], P3[2]); ccdVec3Set(&o->c, center[0], center[1], center[2]); const Quaternion3f& q = tf.getQuatRotation(); const Vec3f& T = tf.getTranslation(); ccdVec3Set(&o->pos, T[0], T[1], T[2]); ccdQuatSet(&o->rot, q.getX(), q.getY(), q.getZ(), q.getW()); ccdQuatInvert2(&o->rot_inv, &o->rot); return o; } void triDeleteGJKObject(void* o_) { ccd_triangle_t* o = static_cast(o_); delete o; } } // details } // fcl fcl-0.5.0/src/narrowphase/narrowphase.cpp000077500000000000000000003470121274356571000204610ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/narrowphase/narrowphase.h" #include "fcl/shape/geometric_shapes_utility.h" #include "fcl/intersect.h" #include namespace fcl { namespace details { // Clamp n to lie within the range [min, max] float clamp(float n, float min, float max) { if (n < min) return min; if (n > max) return max; return n; } // Computes closest points C1 and C2 of S1(s)=P1+s*(Q1-P1) and // S2(t)=P2+t*(Q2-P2), returning s and t. Function result is squared // distance between between S1(s) and S2(t) float closestPtSegmentSegment(Vec3f p1, Vec3f q1, Vec3f p2, Vec3f q2, float &s, float &t, Vec3f &c1, Vec3f &c2) { const float EPSILON = 0.001; Vec3f d1 = q1 - p1; // Direction vector of segment S1 Vec3f d2 = q2 - p2; // Direction vector of segment S2 Vec3f r = p1 - p2; float a = d1.dot(d1); // Squared length of segment S1, always nonnegative float e = d2.dot(d2); // Squared length of segment S2, always nonnegative float f = d2.dot(r); // Check if either or both segments degenerate into points if (a <= EPSILON && e <= EPSILON) { // Both segments degenerate into points s = t = 0.0f; c1 = p1; c2 = p2; Vec3f diff = c1-c2; float res = diff.dot(diff); return res; } if (a <= EPSILON) { // First segment degenerates into a point s = 0.0f; t = f / e; // s = 0 => t = (b*s + f) / e = f / e t = clamp(t, 0.0f, 1.0f); } else { float c = d1.dot(r); if (e <= EPSILON) { // Second segment degenerates into a point t = 0.0f; s = clamp(-c / a, 0.0f, 1.0f); // t = 0 => s = (b*t - c) / a = -c / a } else { // The general nondegenerate case starts here float b = d1.dot(d2); float denom = a*e-b*b; // Always nonnegative // If segments not parallel, compute closest point on L1 to L2 and // clamp to segment S1. Else pick arbitrary s (here 0) if (denom != 0.0f) { std::cerr << "denominator equals zero, using 0 as reference" << std::endl; s = clamp((b*f - c*e) / denom, 0.0f, 1.0f); } else s = 0.0f; // Compute point on L2 closest to S1(s) using // t = Dot((P1 + D1*s) - P2,D2) / Dot(D2,D2) = (b*s + f) / e t = (b*s + f) / e; // //If t in [0,1] done. Else clamp t, recompute s for the new value //of t using s = Dot((P2 + D2*t) - P1,D1) / Dot(D1,D1)= (t*b - c) / a //and clamp s to [0, 1] if(t < 0.0f) { t = 0.0f; s = clamp(-c / a, 0.0f, 1.0f); } else if (t > 1.0f) { t = 1.0f; s = clamp((b - c) / a, 0.0f, 1.0f); } } } c1 = p1 + d1 * s; c2 = p2 + d2 * t; Vec3f diff = c1-c2; float res = diff.dot(diff); return res; } // Computes closest points C1 and C2 of S1(s)=P1+s*(Q1-P1) and // S2(t)=P2+t*(Q2-P2), returning s and t. Function result is squared // distance between between S1(s) and S2(t) bool capsuleCapsuleDistance(const Capsule& s1, const Transform3f& tf1, const Capsule& s2, const Transform3f& tf2, FCL_REAL* dist, Vec3f* p1_res, Vec3f* p2_res) { Vec3f p1(tf1.getTranslation()); Vec3f p2(tf2.getTranslation()); // line segment composes two points. First point is given by the origin, second point is computed by the origin transformed along z. // extension along z-axis means transformation with identity matrix and translation vector z pos Transform3f transformQ1(Vec3f(0,0,s1.lz)); transformQ1 = tf1 * transformQ1; Vec3f q1 = transformQ1.getTranslation(); Transform3f transformQ2(Vec3f(0,0,s2.lz)); transformQ2 = tf2 * transformQ2; Vec3f q2 = transformQ2.getTranslation(); // s and t correspont to the length of the line segment float s, t; Vec3f c1, c2; float result = closestPtSegmentSegment(p1, q1, p2, q2, s, t, c1, c2); *dist = sqrt(result)-s1.radius-s2.radius; // getting directional unit vector Vec3f distVec = c2 -c1; distVec.normalize(); // extend the point to be border of the capsule. // Done by following the directional unit vector for the length of the capsule radius *p1_res = c1 + distVec*s1.radius; distVec = c1-c2; distVec.normalize(); *p2_res = c2 + distVec*s2.radius; return true; } // Compute the point on a line segment that is the closest point on the // segment to to another point. The code is inspired by the explanation // given by Dan Sunday's page: // http://geomalgorithms.com/a02-_lines.html static inline void lineSegmentPointClosestToPoint (const Vec3f &p, const Vec3f &s1, const Vec3f &s2, Vec3f &sp) { Vec3f v = s2 - s1; Vec3f w = p - s1; FCL_REAL c1 = w.dot(v); FCL_REAL c2 = v.dot(v); if (c1 <= 0) { sp = s1; } else if (c2 <= c1) { sp = s2; } else { FCL_REAL b = c1/c2; Vec3f Pb = s1 + v * b; sp = Pb; } } bool sphereCapsuleIntersect(const Sphere& s1, const Transform3f& tf1, const Capsule& s2, const Transform3f& tf2, std::vector* contacts) { Transform3f tf2_inv(tf2); tf2_inv.inverse(); const Vec3f pos1(0., 0., 0.5 * s2.lz); const Vec3f pos2(0., 0., -0.5 * s2.lz); const Vec3f s_c = tf2_inv.transform(tf1.transform(Vec3f())); Vec3f segment_point; lineSegmentPointClosestToPoint (s_c, pos1, pos2, segment_point); Vec3f diff = s_c - segment_point; const FCL_REAL distance = diff.length() - s1.radius - s2.radius; if (distance > 0) return false; const Vec3f local_normal = diff.normalize() * - FCL_REAL(1); if (contacts) { const Vec3f normal = tf2.getQuatRotation().transform(local_normal); const Vec3f point = tf2.transform(segment_point + local_normal * distance); const FCL_REAL penetration_depth = -distance; contacts->push_back(ContactPoint(normal, point, penetration_depth)); } return true; } bool sphereCapsuleDistance(const Sphere& s1, const Transform3f& tf1, const Capsule& s2, const Transform3f& tf2, FCL_REAL* dist, Vec3f* p1, Vec3f* p2) { Transform3f tf2_inv(tf2); tf2_inv.inverse(); Vec3f pos1(0., 0., 0.5 * s2.lz); Vec3f pos2(0., 0., -0.5 * s2.lz); Vec3f s_c = tf2_inv.transform(tf1.transform(Vec3f())); Vec3f segment_point; lineSegmentPointClosestToPoint (s_c, pos1, pos2, segment_point); Vec3f diff = s_c - segment_point; FCL_REAL distance = diff.length() - s1.radius - s2.radius; if(distance <= 0) return false; if(dist) *dist = distance; if(p1 || p2) diff.normalize(); if(p1) { *p1 = s_c - diff * s1.radius; *p1 = inverse(tf1).transform(tf2.transform(*p1)); } if(p2) *p2 = segment_point + diff * s1.radius; return true; } bool sphereSphereIntersect(const Sphere& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, std::vector* contacts) { Vec3f diff = tf2.transform(Vec3f()) - tf1.transform(Vec3f()); FCL_REAL len = diff.length(); if(len > s1.radius + s2.radius) return false; if(contacts) { // If the centers of two sphere are at the same position, the normal is (0, 0, 0). // Otherwise, normal is pointing from center of object 1 to center of object 2 const Vec3f normal = len > 0 ? diff / len : diff; const Vec3f point = tf1.transform(Vec3f()) + diff * s1.radius / (s1.radius + s2.radius); const FCL_REAL penetration_depth = s1.radius + s2.radius - len; contacts->push_back(ContactPoint(normal, point, penetration_depth)); } return true; } bool sphereSphereDistance(const Sphere& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, FCL_REAL* dist, Vec3f* p1, Vec3f* p2) { Vec3f o1 = tf1.getTranslation(); Vec3f o2 = tf2.getTranslation(); Vec3f diff = o1 - o2; FCL_REAL len = diff.length(); if(len > s1.radius + s2.radius) { if(dist) *dist = len - (s1.radius + s2.radius); if(p1) *p1 = inverse(tf1).transform(o1 - diff * (s1.radius / len)); if(p2) *p2 = inverse(tf2).transform(o2 + diff * (s2.radius / len)); return true; } if(dist) *dist = -1; return false; } /** \brief the minimum distance from a point to a line */ FCL_REAL segmentSqrDistance(const Vec3f& from, const Vec3f& to,const Vec3f& p, Vec3f& nearest) { Vec3f diff = p - from; Vec3f v = to - from; FCL_REAL t = v.dot(diff); if(t > 0) { FCL_REAL dotVV = v.dot(v); if(t < dotVV) { t /= dotVV; diff -= v * t; } else { t = 1; diff -= v; } } else t = 0; nearest = from + v * t; return diff.dot(diff); } /// @brief Whether a point's projection is in a triangle bool projectInTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3, const Vec3f& normal, const Vec3f& p) { Vec3f edge1(p2 - p1); Vec3f edge2(p3 - p2); Vec3f edge3(p1 - p3); Vec3f p1_to_p(p - p1); Vec3f p2_to_p(p - p2); Vec3f p3_to_p(p - p3); Vec3f edge1_normal(edge1.cross(normal)); Vec3f edge2_normal(edge2.cross(normal)); Vec3f edge3_normal(edge3.cross(normal)); FCL_REAL r1, r2, r3; r1 = edge1_normal.dot(p1_to_p); r2 = edge2_normal.dot(p2_to_p); r3 = edge3_normal.dot(p3_to_p); if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) || ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) ) return true; return false; } bool sphereTriangleIntersect(const Sphere& s, const Transform3f& tf, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal_) { Vec3f normal = (P2 - P1).cross(P3 - P1); normal.normalize(); const Vec3f& center = tf.getTranslation(); const FCL_REAL& radius = s.radius; FCL_REAL radius_with_threshold = radius + std::numeric_limits::epsilon(); Vec3f p1_to_center = center - P1; FCL_REAL distance_from_plane = p1_to_center.dot(normal); if(distance_from_plane < 0) { distance_from_plane *= -1; normal *= -1; } bool is_inside_contact_plane = (distance_from_plane < radius_with_threshold); bool has_contact = false; Vec3f contact_point; if(is_inside_contact_plane) { if(projectInTriangle(P1, P2, P3, normal, center)) { has_contact = true; contact_point = center - normal * distance_from_plane; } else { FCL_REAL contact_capsule_radius_sqr = radius_with_threshold * radius_with_threshold; Vec3f nearest_on_edge; FCL_REAL distance_sqr; distance_sqr = segmentSqrDistance(P1, P2, center, nearest_on_edge); if(distance_sqr < contact_capsule_radius_sqr) { has_contact = true; contact_point = nearest_on_edge; } distance_sqr = segmentSqrDistance(P2, P3, center, nearest_on_edge); if(distance_sqr < contact_capsule_radius_sqr) { has_contact = true; contact_point = nearest_on_edge; } distance_sqr = segmentSqrDistance(P3, P1, center, nearest_on_edge); if(distance_sqr < contact_capsule_radius_sqr) { has_contact = true; contact_point = nearest_on_edge; } } } if(has_contact) { Vec3f contact_to_center = contact_point - center; FCL_REAL distance_sqr = contact_to_center.sqrLength(); if(distance_sqr < radius_with_threshold * radius_with_threshold) { if(distance_sqr > 0) { FCL_REAL distance = std::sqrt(distance_sqr); if(normal_) *normal_ = normalize(contact_to_center); if(contact_points) *contact_points = contact_point; if(penetration_depth) *penetration_depth = -(radius - distance); } else { if(normal_) *normal_ = -normal; if(contact_points) *contact_points = contact_point; if(penetration_depth) *penetration_depth = -radius; } return true; } } return false; } bool sphereTriangleDistance(const Sphere& sp, const Transform3f& tf, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, FCL_REAL* dist) { // from geometric tools, very different from the collision code. const Vec3f& center = tf.getTranslation(); FCL_REAL radius = sp.radius; Vec3f diff = P1 - center; Vec3f edge0 = P2 - P1; Vec3f edge1 = P3 - P1; FCL_REAL a00 = edge0.sqrLength(); FCL_REAL a01 = edge0.dot(edge1); FCL_REAL a11 = edge1.sqrLength(); FCL_REAL b0 = diff.dot(edge0); FCL_REAL b1 = diff.dot(edge1); FCL_REAL c = diff.sqrLength(); FCL_REAL det = fabs(a00*a11 - a01*a01); FCL_REAL s = a01*b1 - a11*b0; FCL_REAL t = a01*b0 - a00*b1; FCL_REAL sqr_dist; if(s + t <= det) { if(s < 0) { if(t < 0) // region 4 { if(b0 < 0) { t = 0; if(-b0 >= a00) { s = 1; sqr_dist = a00 + 2*b0 + c; } else { s = -b0/a00; sqr_dist = b0*s + c; } } else { s = 0; if(b1 >= 0) { t = 0; sqr_dist = c; } else if(-b1 >= a11) { t = 1; sqr_dist = a11 + 2*b1 + c; } else { t = -b1/a11; sqr_dist = b1*t + c; } } } else // region 3 { s = 0; if(b1 >= 0) { t = 0; sqr_dist = c; } else if(-b1 >= a11) { t = 1; sqr_dist = a11 + 2*b1 + c; } else { t = -b1/a11; sqr_dist = b1*t + c; } } } else if(t < 0) // region 5 { t = 0; if(b0 >= 0) { s = 0; sqr_dist = c; } else if(-b0 >= a00) { s = 1; sqr_dist = a00 + 2*b0 + c; } else { s = -b0/a00; sqr_dist = b0*s + c; } } else // region 0 { // minimum at interior point FCL_REAL inv_det = (1)/det; s *= inv_det; t *= inv_det; sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c; } } else { FCL_REAL tmp0, tmp1, numer, denom; if(s < 0) // region 2 { tmp0 = a01 + b0; tmp1 = a11 + b1; if(tmp1 > tmp0) { numer = tmp1 - tmp0; denom = a00 - 2*a01 + a11; if(numer >= denom) { s = 1; t = 0; sqr_dist = a00 + 2*b0 + c; } else { s = numer/denom; t = 1 - s; sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c; } } else { s = 0; if(tmp1 <= 0) { t = 1; sqr_dist = a11 + 2*b1 + c; } else if(b1 >= 0) { t = 0; sqr_dist = c; } else { t = -b1/a11; sqr_dist = b1*t + c; } } } else if(t < 0) // region 6 { tmp0 = a01 + b1; tmp1 = a00 + b0; if(tmp1 > tmp0) { numer = tmp1 - tmp0; denom = a00 - 2*a01 + a11; if(numer >= denom) { t = 1; s = 0; sqr_dist = a11 + 2*b1 + c; } else { t = numer/denom; s = 1 - t; sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c; } } else { t = 0; if(tmp1 <= 0) { s = 1; sqr_dist = a00 + 2*b0 + c; } else if(b0 >= 0) { s = 0; sqr_dist = c; } else { s = -b0/a00; sqr_dist = b0*s + c; } } } else // region 1 { numer = a11 + b1 - a01 - b0; if(numer <= 0) { s = 0; t = 1; sqr_dist = a11 + 2*b1 + c; } else { denom = a00 - 2*a01 + a11; if(numer >= denom) { s = 1; t = 0; sqr_dist = a00 + 2*b0 + c; } else { s = numer/denom; t = 1 - s; sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c; } } } } // Account for numerical round-off error. if(sqr_dist < 0) sqr_dist = 0; if(sqr_dist > radius * radius) { if(dist) *dist = std::sqrt(sqr_dist) - radius; return true; } else { if(dist) *dist = -1; return false; } } bool sphereTriangleDistance(const Sphere& sp, const Transform3f& tf, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, FCL_REAL* dist, Vec3f* p1, Vec3f* p2) { if(p1 || p2) { Vec3f o = tf.getTranslation(); Project::ProjectResult result; result = Project::projectTriangle(P1, P2, P3, o); if(result.sqr_distance > sp.radius * sp.radius) { if(dist) *dist = std::sqrt(result.sqr_distance) - sp.radius; Vec3f project_p = P1 * result.parameterization[0] + P2 * result.parameterization[1] + P3 * result.parameterization[2]; Vec3f dir = o - project_p; dir.normalize(); if(p1) { *p1 = o - dir * sp.radius; *p1 = inverse(tf).transform(*p1); } if(p2) *p2 = project_p; return true; } else return false; } else { return sphereTriangleDistance(sp, tf, P1, P2, P3, dist); } } bool sphereTriangleDistance(const Sphere& sp, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, FCL_REAL* dist, Vec3f* p1, Vec3f* p2) { bool res = details::sphereTriangleDistance(sp, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P3), dist, p1, p2); if(p2) *p2 = inverse(tf2).transform(*p2); return res; } static inline void lineClosestApproach(const Vec3f& pa, const Vec3f& ua, const Vec3f& pb, const Vec3f& ub, FCL_REAL* alpha, FCL_REAL* beta) { Vec3f p = pb - pa; FCL_REAL uaub = ua.dot(ub); FCL_REAL q1 = ua.dot(p); FCL_REAL q2 = -ub.dot(p); FCL_REAL d = 1 - uaub * uaub; if(d <= (FCL_REAL)(0.0001f)) { *alpha = 0; *beta = 0; } else { d = 1 / d; *alpha = (q1 + uaub * q2) * d; *beta = (uaub * q1 + q2) * d; } } // find all the intersection points between the 2D rectangle with vertices // at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]), // (p[2],p[3]),(p[4],p[5]),(p[6],p[7]). // // the intersection points are returned as x,y pairs in the 'ret' array. // the number of intersection points is returned by the function (this will // be in the range 0 to 8). static int intersectRectQuad2(FCL_REAL h[2], FCL_REAL p[8], FCL_REAL ret[16]) { // q (and r) contain nq (and nr) coordinate points for the current (and // chopped) polygons int nq = 4, nr = 0; FCL_REAL buffer[16]; FCL_REAL* q = p; FCL_REAL* r = ret; for(int dir = 0; dir <= 1; ++dir) { // direction notation: xy[0] = x axis, xy[1] = y axis for(int sign = -1; sign <= 1; sign += 2) { // chop q along the line xy[dir] = sign*h[dir] FCL_REAL* pq = q; FCL_REAL* pr = r; nr = 0; for(int i = nq; i > 0; --i) { // go through all points in q and all lines between adjacent points if(sign * pq[dir] < h[dir]) { // this point is inside the chopping line pr[0] = pq[0]; pr[1] = pq[1]; pr += 2; nr++; if(nr & 8) { q = r; goto done; } } FCL_REAL* nextq = (i > 1) ? pq+2 : q; if((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir])) { // this line crosses the chopping line pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) / (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]); pr[dir] = sign*h[dir]; pr += 2; nr++; if(nr & 8) { q = r; goto done; } } pq += 2; } q = r; r = (q == ret) ? buffer : ret; nq = nr; } } done: if(q != ret) memcpy(ret, q, nr*2*sizeof(FCL_REAL)); return nr; } // given n points in the plane (array p, of size 2*n), generate m points that // best represent the whole set. the definition of 'best' here is not // predetermined - the idea is to select points that give good box-box // collision detection behavior. the chosen point indexes are returned in the // array iret (of size m). 'i0' is always the first entry in the array. // n must be in the range [1..8]. m must be in the range [1..n]. i0 must be // in the range [0..n-1]. static inline void cullPoints2(int n, FCL_REAL p[], int m, int i0, int iret[]) { // compute the centroid of the polygon in cx,cy FCL_REAL a, cx, cy, q; switch(n) { case 1: cx = p[0]; cy = p[1]; break; case 2: cx = 0.5 * (p[0] + p[2]); cy = 0.5 * (p[1] + p[3]); break; default: a = 0; cx = 0; cy = 0; for(int i = 0; i < n-1; ++i) { q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1]; a += q; cx += q*(p[i*2]+p[i*2+2]); cy += q*(p[i*2+1]+p[i*2+3]); } q = p[n*2-2]*p[1] - p[0]*p[n*2-1]; if(std::abs(a+q) > std::numeric_limits::epsilon()) a = 1/(3*(a+q)); else a= 1e18f; cx = a*(cx + q*(p[n*2-2]+p[0])); cy = a*(cy + q*(p[n*2-1]+p[1])); } // compute the angle of each point w.r.t. the centroid FCL_REAL A[8]; for(int i = 0; i < n; ++i) A[i] = atan2(p[i*2+1]-cy,p[i*2]-cx); // search for points that have angles closest to A[i0] + i*(2*pi/m). int avail[8]; for(int i = 0; i < n; ++i) avail[i] = 1; avail[i0] = 0; iret[0] = i0; iret++; const double pi = constants::pi; for(int j = 1; j < m; ++j) { a = j*(2*pi/m) + A[i0]; if (a > pi) a -= 2*pi; FCL_REAL maxdiff= 1e9, diff; *iret = i0; // iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0 for(int i = 0; i < n; ++i) { if(avail[i]) { diff = std::abs(A[i]-a); if(diff > pi) diff = 2*pi - diff; if(diff < maxdiff) { maxdiff = diff; *iret = i; } } } avail[*iret] = 0; iret++; } } int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1, const Vec3f& side2, const Matrix3f& R2, const Vec3f& T2, Vec3f& normal, FCL_REAL* depth, int* return_code, int maxc, std::vector& contacts) { const FCL_REAL fudge_factor = FCL_REAL(1.05); Vec3f normalC; FCL_REAL s, s2, l; int invert_normal, code; Vec3f p = T2 - T1; // get vector from centers of box 1 to box 2, relative to box 1 Vec3f pp = R1.transposeTimes(p); // get pp = p relative to body 1 // get side lengths / 2 Vec3f A = side1 * 0.5; Vec3f B = side2 * 0.5; // Rij is R1'*R2, i.e. the relative rotation between R1 and R2 Matrix3f R = R1.transposeTimes(R2); Matrix3f Q = abs(R); // for all 15 possible separating axes: // * see if the axis separates the boxes. if so, return 0. // * find the depth of the penetration along the separating axis (s2) // * if this is the largest depth so far, record it. // the normal vector will be set to the separating axis with the smallest // depth. note: normalR is set to point to a column of R1 or R2 if that is // the smallest depth normal so far. otherwise normalR is 0 and normalC is // set to a vector relative to body 1. invert_normal is 1 if the sign of // the normal should be flipped. int best_col_id = -1; const Matrix3f* normalR = 0; FCL_REAL tmp = 0; s = - std::numeric_limits::max(); invert_normal = 0; code = 0; // separating axis = u1, u2, u3 tmp = pp[0]; s2 = std::abs(tmp) - (Q.dotX(B) + A[0]); if(s2 > 0) { *return_code = 0; return 0; } if(s2 > s) { s = s2; best_col_id = 0; normalR = &R1; invert_normal = (tmp < 0); code = 1; } tmp = pp[1]; s2 = std::abs(tmp) - (Q.dotY(B) + A[1]); if(s2 > 0) { *return_code = 0; return 0; } if(s2 > s) { s = s2; best_col_id = 1; normalR = &R1; invert_normal = (tmp < 0); code = 2; } tmp = pp[2]; s2 = std::abs(tmp) - (Q.dotZ(B) + A[2]); if(s2 > 0) { *return_code = 0; return 0; } if(s2 > s) { s = s2; best_col_id = 2; normalR = &R1; invert_normal = (tmp < 0); code = 3; } // separating axis = v1, v2, v3 tmp = R2.transposeDotX(p); s2 = std::abs(tmp) - (Q.transposeDotX(A) + B[0]); if(s2 > 0) { *return_code = 0; return 0; } if(s2 > s) { s = s2; best_col_id = 0; normalR = &R2; invert_normal = (tmp < 0); code = 4; } tmp = R2.transposeDotY(p); s2 = std::abs(tmp) - (Q.transposeDotY(A) + B[1]); if(s2 > 0) { *return_code = 0; return 0; } if(s2 > s) { s = s2; best_col_id = 1; normalR = &R2; invert_normal = (tmp < 0); code = 5; } tmp = R2.transposeDotZ(p); s2 = std::abs(tmp) - (Q.transposeDotZ(A) + B[2]); if(s2 > 0) { *return_code = 0; return 0; } if(s2 > s) { s = s2; best_col_id = 2; normalR = &R2; invert_normal = (tmp < 0); code = 6; } FCL_REAL fudge2(1.0e-6); Q += fudge2; Vec3f n; FCL_REAL eps = std::numeric_limits::epsilon(); // separating axis = u1 x (v1,v2,v3) tmp = pp[2] * R(1, 0) - pp[1] * R(2, 0); s2 = std::abs(tmp) - (A[1] * Q(2, 0) + A[2] * Q(1, 0) + B[1] * Q(0, 2) + B[2] * Q(0, 1)); if(s2 > 0) { *return_code = 0; return 0; } n = Vec3f(0, -R(2, 0), R(1, 0)); l = n.length(); if(l > eps) { s2 /= l; if(s2 * fudge_factor > s) { s = s2; best_col_id = -1; normalC = n / l; invert_normal = (tmp < 0); code = 7; } } tmp = pp[2] * R(1, 1) - pp[1] * R(2, 1); s2 = std::abs(tmp) - (A[1] * Q(2, 1) + A[2] * Q(1, 1) + B[0] * Q(0, 2) + B[2] * Q(0, 0)); if(s2 > 0) { *return_code = 0; return 0; } n = Vec3f(0, -R(2, 1), R(1, 1)); l = n.length(); if(l > eps) { s2 /= l; if(s2 * fudge_factor > s) { s = s2; best_col_id = -1; normalC = n / l; invert_normal = (tmp < 0); code = 8; } } tmp = pp[2] * R(1, 2) - pp[1] * R(2, 2); s2 = std::abs(tmp) - (A[1] * Q(2, 2) + A[2] * Q(1, 2) + B[0] * Q(0, 1) + B[1] * Q(0, 0)); if(s2 > 0) { *return_code = 0; return 0; } n = Vec3f(0, -R(2, 2), R(1, 2)); l = n.length(); if(l > eps) { s2 /= l; if(s2 * fudge_factor > s) { s = s2; best_col_id = -1; normalC = n / l; invert_normal = (tmp < 0); code = 9; } } // separating axis = u2 x (v1,v2,v3) tmp = pp[0] * R(2, 0) - pp[2] * R(0, 0); s2 = std::abs(tmp) - (A[0] * Q(2, 0) + A[2] * Q(0, 0) + B[1] * Q(1, 2) + B[2] * Q(1, 1)); if(s2 > 0) { *return_code = 0; return 0; } n = Vec3f(R(2, 0), 0, -R(0, 0)); l = n.length(); if(l > eps) { s2 /= l; if(s2 * fudge_factor > s) { s = s2; best_col_id = -1; normalC = n / l; invert_normal = (tmp < 0); code = 10; } } tmp = pp[0] * R(2, 1) - pp[2] * R(0, 1); s2 = std::abs(tmp) - (A[0] * Q(2, 1) + A[2] * Q(0, 1) + B[0] * Q(1, 2) + B[2] * Q(1, 0)); if(s2 > 0) { *return_code = 0; return 0; } n = Vec3f(R(2, 1), 0, -R(0, 1)); l = n.length(); if(l > eps) { s2 /= l; if(s2 * fudge_factor > s) { s = s2; best_col_id = -1; normalC = n / l; invert_normal = (tmp < 0); code = 11; } } tmp = pp[0] * R(2, 2) - pp[2] * R(0, 2); s2 = std::abs(tmp) - (A[0] * Q(2, 2) + A[2] * Q(0, 2) + B[0] * Q(1, 1) + B[1] * Q(1, 0)); if(s2 > 0) { *return_code = 0; return 0; } n = Vec3f(R(2, 2), 0, -R(0, 2)); l = n.length(); if(l > eps) { s2 /= l; if(s2 * fudge_factor > s) { s = s2; best_col_id = -1; normalC = n / l; invert_normal = (tmp < 0); code = 12; } } // separating axis = u3 x (v1,v2,v3) tmp = pp[1] * R(0, 0) - pp[0] * R(1, 0); s2 = std::abs(tmp) - (A[0] * Q(1, 0) + A[1] * Q(0, 0) + B[1] * Q(2, 2) + B[2] * Q(2, 1)); if(s2 > 0) { *return_code = 0; return 0; } n = Vec3f(-R(1, 0), R(0, 0), 0); l = n.length(); if(l > eps) { s2 /= l; if(s2 * fudge_factor > s) { s = s2; best_col_id = -1; normalC = n / l; invert_normal = (tmp < 0); code = 13; } } tmp = pp[1] * R(0, 1) - pp[0] * R(1, 1); s2 = std::abs(tmp) - (A[0] * Q(1, 1) + A[1] * Q(0, 1) + B[0] * Q(2, 2) + B[2] * Q(2, 0)); if(s2 > 0) { *return_code = 0; return 0; } n = Vec3f(-R(1, 1), R(0, 1), 0); l = n.length(); if(l > eps) { s2 /= l; if(s2 * fudge_factor > s) { s = s2; best_col_id = -1; normalC = n / l; invert_normal = (tmp < 0); code = 14; } } tmp = pp[1] * R(0, 2) - pp[0] * R(1, 2); s2 = std::abs(tmp) - (A[0] * Q(1, 2) + A[1] * Q(0, 2) + B[0] * Q(2, 1) + B[1] * Q(2, 0)); if(s2 > 0) { *return_code = 0; return 0; } n = Vec3f(-R(1, 2), R(0, 2), 0); l = n.length(); if(l > eps) { s2 /= l; if(s2 * fudge_factor > s) { s = s2; best_col_id = -1; normalC = n / l; invert_normal = (tmp < 0); code = 15; } } if (!code) { *return_code = code; return 0; } // if we get to this point, the boxes interpenetrate. compute the normal // in global coordinates. if(best_col_id != -1) normal = normalR->getColumn(best_col_id); else normal = R1 * normalC; if(invert_normal) normal.negate(); *depth = -s; // s is negative when the boxes are in collision // compute contact point(s) if(code > 6) { // an edge from box 1 touches an edge from box 2. // find a point pa on the intersecting edge of box 1 Vec3f pa(T1); FCL_REAL sign; for(int j = 0; j < 3; ++j) { sign = (R1.transposeDot(j, normal) > 0) ? 1 : -1; pa += R1.getColumn(j) * (A[j] * sign); } // find a point pb on the intersecting edge of box 2 Vec3f pb(T2); for(int j = 0; j < 3; ++j) { sign = (R2.transposeDot(j, normal) > 0) ? -1 : 1; pb += R2.getColumn(j) * (B[j] * sign); } FCL_REAL alpha, beta; Vec3f ua(R1.getColumn((code-7)/3)); Vec3f ub(R2.getColumn((code-7)%3)); lineClosestApproach(pa, ua, pb, ub, &alpha, &beta); pa += ua * alpha; pb += ub * beta; // Vec3f pointInWorld((pa + pb) * 0.5); // contacts.push_back(ContactPoint(-normal, pointInWorld, -*depth)); contacts.push_back(ContactPoint(normal,pb,-*depth)); *return_code = code; return 1; } // okay, we have a face-something intersection (because the separating // axis is perpendicular to a face). define face 'a' to be the reference // face (i.e. the normal vector is perpendicular to this) and face 'b' to be // the incident face (the closest face of the other box). const Matrix3f *Ra, *Rb; const Vec3f *pa, *pb, *Sa, *Sb; if(code <= 3) { Ra = &R1; Rb = &R2; pa = &T1; pb = &T2; Sa = &A; Sb = &B; } else { Ra = &R2; Rb = &R1; pa = &T2; pb = &T1; Sa = &B; Sb = &A; } // nr = normal vector of reference face dotted with axes of incident box. // anr = absolute values of nr. Vec3f normal2, nr, anr; if(code <= 3) normal2 = normal; else normal2 = -normal; nr = Rb->transposeTimes(normal2); anr = abs(nr); // find the largest compontent of anr: this corresponds to the normal // for the indident face. the other axis numbers of the indicent face // are stored in a1,a2. int lanr, a1, a2; if(anr[1] > anr[0]) { if(anr[1] > anr[2]) { a1 = 0; lanr = 1; a2 = 2; } else { a1 = 0; a2 = 1; lanr = 2; } } else { if(anr[0] > anr[2]) { lanr = 0; a1 = 1; a2 = 2; } else { a1 = 0; a2 = 1; lanr = 2; } } // compute center point of incident face, in reference-face coordinates Vec3f center; if(nr[lanr] < 0) center = (*pb) - (*pa) + Rb->getColumn(lanr) * ((*Sb)[lanr]); else center = (*pb) - (*pa) - Rb->getColumn(lanr) * ((*Sb)[lanr]); // find the normal and non-normal axis numbers of the reference box int codeN, code1, code2; if(code <= 3) codeN = code-1; else codeN = code-4; if(codeN == 0) { code1 = 1; code2 = 2; } else if(codeN == 1) { code1 = 0; code2 = 2; } else { code1 = 0; code2 = 1; } // find the four corners of the incident face, in reference-face coordinates FCL_REAL quad[8]; // 2D coordinate of incident face (x,y pairs) FCL_REAL c1, c2, m11, m12, m21, m22; c1 = Ra->transposeDot(code1, center); c2 = Ra->transposeDot(code2, center); // optimize this? - we have already computed this data above, but it is not // stored in an easy-to-index format. for now it's quicker just to recompute // the four dot products. Vec3f tempRac = Ra->getColumn(code1); m11 = Rb->transposeDot(a1, tempRac); m12 = Rb->transposeDot(a2, tempRac); tempRac = Ra->getColumn(code2); m21 = Rb->transposeDot(a1, tempRac); m22 = Rb->transposeDot(a2, tempRac); FCL_REAL k1 = m11 * (*Sb)[a1]; FCL_REAL k2 = m21 * (*Sb)[a1]; FCL_REAL k3 = m12 * (*Sb)[a2]; FCL_REAL k4 = m22 * (*Sb)[a2]; quad[0] = c1 - k1 - k3; quad[1] = c2 - k2 - k4; quad[2] = c1 - k1 + k3; quad[3] = c2 - k2 + k4; quad[4] = c1 + k1 + k3; quad[5] = c2 + k2 + k4; quad[6] = c1 + k1 - k3; quad[7] = c2 + k2 - k4; // find the size of the reference face FCL_REAL rect[2]; rect[0] = (*Sa)[code1]; rect[1] = (*Sa)[code2]; // intersect the incident and reference faces FCL_REAL ret[16]; int n_intersect = intersectRectQuad2(rect, quad, ret); if(n_intersect < 1) { *return_code = code; return 0; } // this should never happen // convert the intersection points into reference-face coordinates, // and compute the contact position and depth for each point. only keep // those points that have a positive (penetrating) depth. delete points in // the 'ret' array as necessary so that 'point' and 'ret' correspond. Vec3f points[8]; // penetrating contact points FCL_REAL dep[8]; // depths for those points FCL_REAL det1 = 1.f/(m11*m22 - m12*m21); m11 *= det1; m12 *= det1; m21 *= det1; m22 *= det1; int cnum = 0; // number of penetrating contact points found for(int j = 0; j < n_intersect; ++j) { FCL_REAL k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2); FCL_REAL k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2); points[cnum] = center + Rb->getColumn(a1) * k1 + Rb->getColumn(a2) * k2; dep[cnum] = (*Sa)[codeN] - normal2.dot(points[cnum]); if(dep[cnum] >= 0) { ret[cnum*2] = ret[j*2]; ret[cnum*2+1] = ret[j*2+1]; cnum++; } } if(cnum < 1) { *return_code = code; return 0; } // this should never happen // we can't generate more contacts than we actually have if(maxc > cnum) maxc = cnum; if(maxc < 1) maxc = 1; if(cnum <= maxc) { if(code<4) { // we have less contacts than we need, so we use them all for(int j = 0; j < cnum; ++j) { Vec3f pointInWorld = points[j] + (*pa); contacts.push_back(ContactPoint(normal, pointInWorld, -dep[j])); } } else { // we have less contacts than we need, so we use them all for(int j = 0; j < cnum; ++j) { Vec3f pointInWorld = points[j] + (*pa) - normal * dep[j]; contacts.push_back(ContactPoint(normal, pointInWorld, -dep[j])); } } } else { // we have more contacts than are wanted, some of them must be culled. // find the deepest point, it is always the first contact. int i1 = 0; FCL_REAL maxdepth = dep[0]; for(int i = 1; i < cnum; ++i) { if(dep[i] > maxdepth) { maxdepth = dep[i]; i1 = i; } } int iret[8]; cullPoints2(cnum, ret, maxc, i1, iret); for(int j = 0; j < maxc; ++j) { Vec3f posInWorld = points[iret[j]] + (*pa); if(code < 4) contacts.push_back(ContactPoint(normal, posInWorld, -dep[iret[j]])); else contacts.push_back(ContactPoint(normal, posInWorld - normal * dep[iret[j]], -dep[iret[j]])); } cnum = maxc; } *return_code = code; return cnum; } bool boxBoxIntersect(const Box& s1, const Transform3f& tf1, const Box& s2, const Transform3f& tf2, std::vector* contacts_) { std::vector contacts; int return_code; Vec3f normal; FCL_REAL depth; /* int cnum = */ boxBox2(s1.side, tf1.getRotation(), tf1.getTranslation(), s2.side, tf2.getRotation(), tf2.getTranslation(), normal, &depth, &return_code, 4, contacts); if(contacts_) *contacts_ = contacts; return return_code != 0; } template T halfspaceIntersectTolerance() { return 0; } template<> float halfspaceIntersectTolerance() { return 0.0001f; } template<> double halfspaceIntersectTolerance() { return 0.0000001; } bool sphereHalfspaceIntersect(const Sphere& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) { const Halfspace new_s2 = transform(s2, tf2); const Vec3f& center = tf1.getTranslation(); const FCL_REAL depth = s1.radius - new_s2.signedDistance(center); if (depth >= 0) { if (contacts) { const Vec3f normal = -new_s2.n; // pointing from s1 to s2 const Vec3f point = center - new_s2.n * s1.radius + new_s2.n * (depth * 0.5); const FCL_REAL penetration_depth = depth; contacts->push_back(ContactPoint(normal, point, penetration_depth)); } return true; } else { return false; } } bool ellipsoidHalfspaceIntersect(const Ellipsoid& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) { // We first compute a single contact in the ellipsoid coordinates, tf1, then // will transform it to the world frame. So we use a new halfspace that is // expressed in the ellipsoid coordinates. const Halfspace& new_s2 = transform(s2, inverse(tf1) * tf2); // Compute distance between the ellipsoid's center and a contact plane, whose // normal is equal to the halfspace's normal. const Vec3f normal2(std::pow(new_s2.n[0], 2), std::pow(new_s2.n[1], 2), std::pow(new_s2.n[2], 2)); const Vec3f radii2(std::pow(s1.radii[0], 2), std::pow(s1.radii[1], 2), std::pow(s1.radii[2], 2)); const FCL_REAL center_to_contact_plane = std::sqrt(normal2.dot(radii2)); // Depth is the distance between the contact plane and the halfspace. const FCL_REAL depth = center_to_contact_plane + new_s2.d; if (depth >= 0) { if (contacts) { // Transform the results to the world coordinates. const Vec3f normal = tf1.getRotation() * -new_s2.n; // pointing from s1 to s2 const Vec3f support_vector = (1.0/center_to_contact_plane) * Vec3f(radii2[0]*new_s2.n[0], radii2[1]*new_s2.n[1], radii2[2]*new_s2.n[2]); const Vec3f point_in_halfspace_coords = support_vector * (0.5 * depth / new_s2.n.dot(support_vector) - 1.0); const Vec3f point = tf1.transform(point_in_halfspace_coords); // roughly speaking, a middle point of the intersecting volume const FCL_REAL penetration_depth = depth; contacts->push_back(ContactPoint(normal, point, penetration_depth)); } return true; } else { return false; } } /// @brief box half space, a, b, c = +/- edge size /// n^T * (R(o + a v1 + b v2 + c v3) + T) <= d /// so (R^T n) (a v1 + b v2 + c v3) + n * T <= d /// check whether d - n * T - (R^T n) (a v1 + b v2 + c v3) >= 0 for some a, b, c /// the max value of left side is d - n * T + |(R^T n) (a v1 + b v2 + c v3)|, check that is enough bool boxHalfspaceIntersect(const Box& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2) { Halfspace new_s2 = transform(s2, tf2); const Matrix3f& R = tf1.getRotation(); const Vec3f& T = tf1.getTranslation(); Vec3f Q = R.transposeTimes(new_s2.n); Vec3f A(Q[0] * s1.side[0], Q[1] * s1.side[1], Q[2] * s1.side[2]); Vec3f B = abs(A); FCL_REAL depth = 0.5 * (B[0] + B[1] + B[2]) - new_s2.signedDistance(T); return (depth >= 0); } bool boxHalfspaceIntersect(const Box& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) { if(!contacts) { return boxHalfspaceIntersect(s1, tf1, s2, tf2); } else { const Halfspace new_s2 = transform(s2, tf2); const Matrix3f& R = tf1.getRotation(); const Vec3f& T = tf1.getTranslation(); Vec3f Q = R.transposeTimes(new_s2.n); Vec3f A(Q[0] * s1.side[0], Q[1] * s1.side[1], Q[2] * s1.side[2]); Vec3f B = abs(A); FCL_REAL depth = 0.5 * (B[0] + B[1] + B[2]) - new_s2.signedDistance(T); if(depth < 0) return false; Vec3f axis[3]; axis[0] = R.getColumn(0); axis[1] = R.getColumn(1); axis[2] = R.getColumn(2); /// find deepest point Vec3f p(T); int sign = 0; if(std::abs(Q[0] - 1) < halfspaceIntersectTolerance() || std::abs(Q[0] + 1) < halfspaceIntersectTolerance()) { sign = (A[0] > 0) ? -1 : 1; p += axis[0] * (0.5 * s1.side[0] * sign); } else if(std::abs(Q[1] - 1) < halfspaceIntersectTolerance() || std::abs(Q[1] + 1) < halfspaceIntersectTolerance()) { sign = (A[1] > 0) ? -1 : 1; p += axis[1] * (0.5 * s1.side[1] * sign); } else if(std::abs(Q[2] - 1) < halfspaceIntersectTolerance() || std::abs(Q[2] + 1) < halfspaceIntersectTolerance()) { sign = (A[2] > 0) ? -1 : 1; p += axis[2] * (0.5 * s1.side[2] * sign); } else { for(std::size_t i = 0; i < 3; ++i) { sign = (A[i] > 0) ? -1 : 1; p += axis[i] * (0.5 * s1.side[i] * sign); } } /// compute the contact point from the deepest point if (contacts) { const Vec3f normal = -new_s2.n; const Vec3f point = p + new_s2.n * (depth * 0.5); const FCL_REAL penetration_depth = depth; contacts->push_back(ContactPoint(normal, point, penetration_depth)); } return true; } } bool capsuleHalfspaceIntersect(const Capsule& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) { Halfspace new_s2 = transform(s2, tf2); const Matrix3f& R = tf1.getRotation(); const Vec3f& T = tf1.getTranslation(); Vec3f dir_z = R.getColumn(2); FCL_REAL cosa = dir_z.dot(new_s2.n); if(std::abs(cosa) < halfspaceIntersectTolerance()) { FCL_REAL signed_dist = new_s2.signedDistance(T); FCL_REAL depth = s1.radius - signed_dist; if(depth < 0) return false; if (contacts) { const Vec3f normal = -new_s2.n; const Vec3f point = T + new_s2.n * (0.5 * depth - s1.radius); const FCL_REAL penetration_depth = depth; contacts->push_back(ContactPoint(normal, point, penetration_depth)); } return true; } else { int sign = (cosa > 0) ? -1 : 1; Vec3f p = T + dir_z * (s1.lz * 0.5 * sign); FCL_REAL signed_dist = new_s2.signedDistance(p); FCL_REAL depth = s1.radius - signed_dist; if(depth < 0) return false; if (contacts) { const Vec3f normal = -new_s2.n; const Vec3f point = p - new_s2.n * s1.radius + new_s2.n * (0.5 * depth); // deepest point const FCL_REAL penetration_depth = depth; contacts->push_back(ContactPoint(normal, point, penetration_depth)); } return true; } } bool cylinderHalfspaceIntersect(const Cylinder& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) { Halfspace new_s2 = transform(s2, tf2); const Matrix3f& R = tf1.getRotation(); const Vec3f& T = tf1.getTranslation(); Vec3f dir_z = R.getColumn(2); FCL_REAL cosa = dir_z.dot(new_s2.n); if(cosa < halfspaceIntersectTolerance()) { FCL_REAL signed_dist = new_s2.signedDistance(T); FCL_REAL depth = s1.radius - signed_dist; if(depth < 0) return false; if (contacts) { const Vec3f normal = -new_s2.n; const Vec3f point = T + new_s2.n * (0.5 * depth - s1.radius); const FCL_REAL penetration_depth = depth; contacts->push_back(ContactPoint(normal, point, penetration_depth)); } return true; } else { Vec3f C = dir_z * cosa - new_s2.n; if(std::abs(cosa + 1) < halfspaceIntersectTolerance() || std::abs(cosa - 1) < halfspaceIntersectTolerance()) C = Vec3f(0, 0, 0); else { FCL_REAL s = C.length(); s = s1.radius / s; C *= s; } int sign = (cosa > 0) ? -1 : 1; // deepest point Vec3f p = T + dir_z * (s1.lz * 0.5 * sign) + C; FCL_REAL depth = -new_s2.signedDistance(p); if(depth < 0) return false; else { if (contacts) { const Vec3f normal = -new_s2.n; const Vec3f point = p + new_s2.n * (0.5 * depth); const FCL_REAL penetration_depth = depth; contacts->push_back(ContactPoint(normal, point, penetration_depth)); } return true; } } } bool coneHalfspaceIntersect(const Cone& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) { Halfspace new_s2 = transform(s2, tf2); const Matrix3f& R = tf1.getRotation(); const Vec3f& T = tf1.getTranslation(); Vec3f dir_z = R.getColumn(2); FCL_REAL cosa = dir_z.dot(new_s2.n); if(cosa < halfspaceIntersectTolerance()) { FCL_REAL signed_dist = new_s2.signedDistance(T); FCL_REAL depth = s1.radius - signed_dist; if(depth < 0) return false; else { if (contacts) { const Vec3f normal = -new_s2.n; const Vec3f point = T - dir_z * (s1.lz * 0.5) + new_s2.n * (0.5 * depth - s1.radius); const FCL_REAL penetration_depth = depth; contacts->push_back(ContactPoint(normal, point, penetration_depth)); } return true; } } else { Vec3f C = dir_z * cosa - new_s2.n; if(std::abs(cosa + 1) < halfspaceIntersectTolerance() || std::abs(cosa - 1) < halfspaceIntersectTolerance()) C = Vec3f(0, 0, 0); else { FCL_REAL s = C.length(); s = s1.radius / s; C *= s; } Vec3f p1 = T + dir_z * (0.5 * s1.lz); Vec3f p2 = T - dir_z * (0.5 * s1.lz) + C; FCL_REAL d1 = new_s2.signedDistance(p1); FCL_REAL d2 = new_s2.signedDistance(p2); if(d1 > 0 && d2 > 0) return false; else { if (contacts) { const FCL_REAL penetration_depth = -std::min(d1, d2); const Vec3f normal = -new_s2.n; const Vec3f point = ((d1 < d2) ? p1 : p2) + new_s2.n * (0.5 * penetration_depth); contacts->push_back(ContactPoint(normal, point, penetration_depth)); } return true; } } } bool convexHalfspaceIntersect(const Convex& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) { Halfspace new_s2 = transform(s2, tf2); Vec3f v; FCL_REAL depth = std::numeric_limits::max(); for(int i = 0; i < s1.num_points; ++i) { Vec3f p = tf1.transform(s1.points[i]); FCL_REAL d = new_s2.signedDistance(p); if(d < depth) { depth = d; v = p; } } if(depth <= 0) { if(contact_points) *contact_points = v - new_s2.n * (0.5 * depth); if(penetration_depth) *penetration_depth = depth; if(normal) *normal = -new_s2.n; return true; } else return false; } bool halfspaceTriangleIntersect(const Halfspace& s1, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) { Halfspace new_s1 = transform(s1, tf1); Vec3f v = tf2.transform(P1); FCL_REAL depth = new_s1.signedDistance(v); Vec3f p = tf2.transform(P2); FCL_REAL d = new_s1.signedDistance(p); if(d < depth) { depth = d; v = p; } p = tf2.transform(P3); d = new_s1.signedDistance(p); if(d < depth) { depth = d; v = p; } if(depth <= 0) { if(penetration_depth) *penetration_depth = -depth; if(normal) *normal = new_s1.n; if(contact_points) *contact_points = v - new_s1.n * (0.5 * depth); return true; } else return false; } /// @brief return whether plane collides with halfspace /// if the separation plane of the halfspace is parallel with the plane /// return code 1, if the plane's normal is the same with halfspace's normal and plane is inside halfspace, also return plane in pl /// return code 2, if the plane's normal is oppositie to the halfspace's normal and plane is inside halfspace, also return plane in pl /// plane is outside halfspace, collision-free /// if not parallel /// return the intersection ray, return code 3. ray origin is p and direction is d bool planeHalfspaceIntersect(const Plane& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, Plane& pl, Vec3f& p, Vec3f& d, FCL_REAL& penetration_depth, int& ret) { Plane new_s1 = transform(s1, tf1); Halfspace new_s2 = transform(s2, tf2); ret = 0; Vec3f dir = (new_s1.n).cross(new_s2.n); FCL_REAL dir_norm = dir.sqrLength(); if(dir_norm < std::numeric_limits::epsilon()) // parallel { if((new_s1.n).dot(new_s2.n) > 0) { if(new_s1.d < new_s2.d) { penetration_depth = new_s2.d - new_s1.d; ret = 1; pl = new_s1; return true; } else return false; } else { if(new_s1.d + new_s2.d > 0) return false; else { penetration_depth = -(new_s1.d + new_s2.d); ret = 2; pl = new_s1; return true; } } } Vec3f n = new_s2.n * new_s1.d - new_s1.n * new_s2.d; Vec3f origin = n.cross(dir); origin *= (1.0 / dir_norm); p = origin; d = dir; ret = 3; penetration_depth = std::numeric_limits::max(); return true; } ///@ brief return whether two halfspace intersect /// if the separation planes of the two halfspaces are parallel /// return code 1, if two halfspaces' normal are same and s1 is in s2, also return s1 in s; /// return code 2, if two halfspaces' normal are same and s2 is in s1, also return s2 in s; /// return code 3, if two halfspaces' normal are opposite and s1 and s2 are into each other; /// collision free, if two halfspaces' are separate; /// if the separation planes of the two halfspaces are not parallel, return intersection ray, return code 4. ray origin is p and direction is d /// collision free return code 0 bool halfspaceIntersect(const Halfspace& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, Vec3f& p, Vec3f& d, Halfspace& s, FCL_REAL& penetration_depth, int& ret) { Halfspace new_s1 = transform(s1, tf1); Halfspace new_s2 = transform(s2, tf2); ret = 0; Vec3f dir = (new_s1.n).cross(new_s2.n); FCL_REAL dir_norm = dir.sqrLength(); if(dir_norm < std::numeric_limits::epsilon()) // parallel { if((new_s1.n).dot(new_s2.n) > 0) { if(new_s1.d < new_s2.d) // s1 is inside s2 { ret = 1; penetration_depth = std::numeric_limits::max(); s = new_s1; } else // s2 is inside s1 { ret = 2; penetration_depth = std::numeric_limits::max(); s = new_s2; } return true; } else { if(new_s1.d + new_s2.d > 0) // not collision return false; else // in each other { ret = 3; penetration_depth = -(new_s1.d + new_s2.d); return true; } } } Vec3f n = new_s2.n * new_s1.d - new_s1.n * new_s2.d; Vec3f origin = n.cross(dir); origin *= (1.0 / dir_norm); p = origin; d = dir; ret = 4; penetration_depth = std::numeric_limits::max(); return true; } template T planeIntersectTolerance() { return 0; } template<> double planeIntersectTolerance() { return 0.0000001; } template<> float planeIntersectTolerance() { return 0.0001; } bool spherePlaneIntersect(const Sphere& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) { const Plane new_s2 = transform(s2, tf2); const Vec3f& center = tf1.getTranslation(); const FCL_REAL signed_dist = new_s2.signedDistance(center); const FCL_REAL depth = - std::abs(signed_dist) + s1.radius; if(depth >= 0) { if (contacts) { const Vec3f normal = (signed_dist > 0) ? -new_s2.n : new_s2.n; const Vec3f point = center - new_s2.n * signed_dist; const FCL_REAL penetration_depth = depth; contacts->push_back(ContactPoint(normal, point, penetration_depth)); } return true; } else { return false; } } bool ellipsoidPlaneIntersect(const Ellipsoid& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) { // We first compute a single contact in the ellipsoid coordinates, tf1, then // will transform it to the world frame. So we use a new plane that is // expressed in the ellipsoid coordinates. const Plane& new_s2 = transform(s2, inverse(tf1) * tf2); // Compute distance between the ellipsoid's center and a contact plane, whose // normal is equal to the plane's normal. const Vec3f normal2(std::pow(new_s2.n[0], 2), std::pow(new_s2.n[1], 2), std::pow(new_s2.n[2], 2)); const Vec3f radii2(std::pow(s1.radii[0], 2), std::pow(s1.radii[1], 2), std::pow(s1.radii[2], 2)); const FCL_REAL center_to_contact_plane = std::sqrt(normal2.dot(radii2)); const FCL_REAL signed_dist = -new_s2.d; // Depth is the distance between the contact plane and the given plane. const FCL_REAL depth = center_to_contact_plane - std::abs(signed_dist); if (depth >= 0) { if (contacts) { // Transform the results to the world coordinates. const Vec3f normal = (signed_dist > 0) ? tf1.getRotation() * -new_s2.n : tf1.getRotation() * new_s2.n; // pointing from the ellipsoid's center to the plane const Vec3f support_vector = (1.0/center_to_contact_plane) * Vec3f(radii2[0]*new_s2.n[0], radii2[1]*new_s2.n[1], radii2[2]*new_s2.n[2]); const Vec3f point_in_plane_coords = support_vector * (depth / new_s2.n.dot(support_vector) - 1.0); const Vec3f point = (signed_dist > 0) ? tf1.transform(point_in_plane_coords) : tf1.transform(-point_in_plane_coords); // a middle point of the intersecting volume const FCL_REAL penetration_depth = depth; contacts->push_back(ContactPoint(normal, point, penetration_depth)); } return true; } else { return false; } } /// @brief box half space, a, b, c = +/- edge size /// n^T * (R(o + a v1 + b v2 + c v3) + T) ~ d /// so (R^T n) (a v1 + b v2 + c v3) + n * T ~ d /// check whether d - n * T - (R^T n) (a v1 + b v2 + c v3) >= 0 for some a, b, c and <=0 for some a, b, c /// so need to check whether |d - n * T| <= |(R^T n)(a v1 + b v2 + c v3)|, the reason is as follows: /// (R^T n) (a v1 + b v2 + c v3) can get |(R^T n) (a v1 + b v2 + c v3)| for one a, b, c. /// if |d - n * T| <= |(R^T n)(a v1 + b v2 + c v3)| then can get both positive and negative value on the right side. bool boxPlaneIntersect(const Box& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) { Plane new_s2 = transform(s2, tf2); const Matrix3f& R = tf1.getRotation(); const Vec3f& T = tf1.getTranslation(); Vec3f Q = R.transposeTimes(new_s2.n); Vec3f A(Q[0] * s1.side[0], Q[1] * s1.side[1], Q[2] * s1.side[2]); Vec3f B = abs(A); FCL_REAL signed_dist = new_s2.signedDistance(T); FCL_REAL depth = 0.5 * (B[0] + B[1] + B[2]) - std::abs(signed_dist); if(depth < 0) return false; Vec3f axis[3]; axis[0] = R.getColumn(0); axis[1] = R.getColumn(1); axis[2] = R.getColumn(2); // find the deepest point Vec3f p = T; // when center is on the positive side of the plane, use a, b, c make (R^T n) (a v1 + b v2 + c v3) the minimum // otherwise, use a, b, c make (R^T n) (a v1 + b v2 + c v3) the maximum int sign = (signed_dist > 0) ? 1 : -1; if(std::abs(Q[0] - 1) < planeIntersectTolerance() || std::abs(Q[0] + 1) < planeIntersectTolerance()) { int sign2 = (A[0] > 0) ? -1 : 1; sign2 *= sign; p += axis[0] * (0.5 * s1.side[0] * sign2); } else if(std::abs(Q[1] - 1) < planeIntersectTolerance() || std::abs(Q[1] + 1) < planeIntersectTolerance()) { int sign2 = (A[1] > 0) ? -1 : 1; sign2 *= sign; p += axis[1] * (0.5 * s1.side[1] * sign2); } else if(std::abs(Q[2] - 1) < planeIntersectTolerance() || std::abs(Q[2] + 1) < planeIntersectTolerance()) { int sign2 = (A[2] > 0) ? -1 : 1; sign2 *= sign; p += axis[2] * (0.5 * s1.side[2] * sign2); } else { for(std::size_t i = 0; i < 3; ++i) { int sign2 = (A[i] > 0) ? -1 : 1; sign2 *= sign; p += axis[i] * (0.5 * s1.side[i] * sign2); } } // compute the contact point by project the deepest point onto the plane if (contacts) { const Vec3f normal = (signed_dist > 0) ? -new_s2.n : new_s2.n; const Vec3f point = p - new_s2.n * new_s2.signedDistance(p); const FCL_REAL penetration_depth = depth; contacts->push_back(ContactPoint(normal, point, penetration_depth)); } return true; } bool capsulePlaneIntersect(const Capsule& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2) { Plane new_s2 = transform(s2, tf2); const Matrix3f& R = tf1.getRotation(); const Vec3f& T = tf1.getTranslation(); Vec3f dir_z = R.getColumn(2); Vec3f p1 = T + dir_z * (0.5 * s1.lz); Vec3f p2 = T - dir_z * (0.5 * s1.lz); FCL_REAL d1 = new_s2.signedDistance(p1); FCL_REAL d2 = new_s2.signedDistance(p2); // two end points on different side of the plane if(d1 * d2 <= 0) return true; // two end points on the same side of the plane, but the end point spheres might intersect the plane return (std::abs(d1) <= s1.radius) || (std::abs(d2) <= s1.radius); } bool capsulePlaneIntersect(const Capsule& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) { if(!contacts) { return capsulePlaneIntersect(s1, tf1, s2, tf2); } else { Plane new_s2 = transform(s2, tf2); const Matrix3f& R = tf1.getRotation(); const Vec3f& T = tf1.getTranslation(); Vec3f dir_z = R.getColumn(2); Vec3f p1 = T + dir_z * (0.5 * s1.lz); Vec3f p2 = T - dir_z * (0.5 * s1.lz); FCL_REAL d1 = new_s2.signedDistance(p1); FCL_REAL d2 = new_s2.signedDistance(p2); FCL_REAL abs_d1 = std::abs(d1); FCL_REAL abs_d2 = std::abs(d2); // two end points on different side of the plane // the contact point is the intersect of axis with the plane // the normal is the direction to avoid intersection // the depth is the minimum distance to resolve the collision if(d1 * d2 < -planeIntersectTolerance()) { if(abs_d1 < abs_d2) { if (contacts) { const Vec3f normal = (d1 < 0) ? -new_s2.n : new_s2.n; const Vec3f point = p1 * (abs_d2 / (abs_d1 + abs_d2)) + p2 * (abs_d1 / (abs_d1 + abs_d2)); const FCL_REAL penetration_depth = abs_d1 + s1.radius; contacts->push_back(ContactPoint(normal, point, penetration_depth)); } } else { if (contacts) { const Vec3f normal = (d2 < 0) ? -new_s2.n : new_s2.n; const Vec3f point = p1 * (abs_d2 / (abs_d1 + abs_d2)) + p2 * (abs_d1 / (abs_d1 + abs_d2)); const FCL_REAL penetration_depth = abs_d2 + s1.radius; contacts->push_back(ContactPoint(normal, point, penetration_depth)); } } return true; } if(abs_d1 > s1.radius && abs_d2 > s1.radius) { return false; } else { if (contacts) { const Vec3f normal = (d1 < 0) ? new_s2.n : -new_s2.n; const FCL_REAL penetration_depth = s1.radius - std::min(abs_d1, abs_d2); Vec3f point; if(abs_d1 <= s1.radius && abs_d2 <= s1.radius) { const Vec3f c1 = p1 - new_s2.n * d2; const Vec3f c2 = p2 - new_s2.n * d1; point = (c1 + c2) * 0.5; } else if(abs_d1 <= s1.radius) { const Vec3f c = p1 - new_s2.n * d1; point = c; } else if(abs_d2 <= s1.radius) { const Vec3f c = p2 - new_s2.n * d2; point = c; } contacts->push_back(ContactPoint(normal, point, penetration_depth)); } return true; } } } /// @brief cylinder-plane intersect /// n^T (R (r * cosa * v1 + r * sina * v2 + h * v3) + T) ~ d /// need one point to be positive and one to be negative /// (n^T * v3) * h + n * T -d + r * (cosa * (n^T * R * v1) + sina * (n^T * R * v2)) ~ 0 /// (n^T * v3) * h + r * (cosa * (n^T * R * v1) + sina * (n^T * R * v2)) + n * T - d ~ 0 bool cylinderPlaneIntersect(const Cylinder& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2) { Plane new_s2 = transform(s2, tf2); const Matrix3f& R = tf1.getRotation(); const Vec3f& T = tf1.getTranslation(); Vec3f Q = R.transposeTimes(new_s2.n); FCL_REAL term = std::abs(Q[2]) * s1.lz + s1.radius * std::sqrt(Q[0] * Q[0] + Q[1] * Q[1]); FCL_REAL dist = new_s2.distance(T); FCL_REAL depth = term - dist; if(depth < 0) return false; else return true; } bool cylinderPlaneIntersect(const Cylinder& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) { if(!contacts) { return cylinderPlaneIntersect(s1, tf1, s2, tf2); } else { Plane new_s2 = transform(s2, tf2); const Matrix3f& R = tf1.getRotation(); const Vec3f& T = tf1.getTranslation(); Vec3f dir_z = R.getColumn(2); FCL_REAL cosa = dir_z.dot(new_s2.n); if(std::abs(cosa) < planeIntersectTolerance()) { FCL_REAL d = new_s2.signedDistance(T); FCL_REAL depth = s1.radius - std::abs(d); if(depth < 0) return false; else { if (contacts) { const Vec3f normal = (d < 0) ? new_s2.n : -new_s2.n; const Vec3f point = T - new_s2.n * d; const FCL_REAL penetration_depth = depth; contacts->push_back(ContactPoint(normal, point, penetration_depth)); } return true; } } else { Vec3f C = dir_z * cosa - new_s2.n; if(std::abs(cosa + 1) < planeIntersectTolerance() || std::abs(cosa - 1) < planeIntersectTolerance()) C = Vec3f(0, 0, 0); else { FCL_REAL s = C.length(); s = s1.radius / s; C *= s; } Vec3f p1 = T + dir_z * (0.5 * s1.lz); Vec3f p2 = T - dir_z * (0.5 * s1.lz); Vec3f c1, c2; if(cosa > 0) { c1 = p1 - C; c2 = p2 + C; } else { c1 = p1 + C; c2 = p2 - C; } FCL_REAL d1 = new_s2.signedDistance(c1); FCL_REAL d2 = new_s2.signedDistance(c2); if(d1 * d2 <= 0) { FCL_REAL abs_d1 = std::abs(d1); FCL_REAL abs_d2 = std::abs(d2); if(abs_d1 > abs_d2) { if (contacts) { const Vec3f normal = (d2 < 0) ? -new_s2.n : new_s2.n; const Vec3f point = c2 - new_s2.n * d2; const FCL_REAL penetration_depth = abs_d2; contacts->push_back(ContactPoint(normal, point, penetration_depth)); } } else { if (contacts) { const Vec3f normal = (d1 < 0) ? -new_s2.n : new_s2.n; const Vec3f point = c1 - new_s2.n * d1; const FCL_REAL penetration_depth = abs_d1; contacts->push_back(ContactPoint(normal, point, penetration_depth)); } } return true; } else { return false; } } } } bool conePlaneIntersect(const Cone& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) { Plane new_s2 = transform(s2, tf2); const Matrix3f& R = tf1.getRotation(); const Vec3f& T = tf1.getTranslation(); Vec3f dir_z = R.getColumn(2); FCL_REAL cosa = dir_z.dot(new_s2.n); if(std::abs(cosa) < planeIntersectTolerance()) { FCL_REAL d = new_s2.signedDistance(T); FCL_REAL depth = s1.radius - std::abs(d); if(depth < 0) return false; else { if (contacts) { const Vec3f normal = (d < 0) ? new_s2.n : -new_s2.n; const Vec3f point = T - dir_z * (0.5 * s1.lz) + dir_z * (0.5 * depth / s1.radius * s1.lz) - new_s2.n * d; const FCL_REAL penetration_depth = depth; contacts->push_back(ContactPoint(normal, point, penetration_depth)); } return true; } } else { Vec3f C = dir_z * cosa - new_s2.n; if(std::abs(cosa + 1) < planeIntersectTolerance() || std::abs(cosa - 1) < planeIntersectTolerance()) C = Vec3f(0, 0, 0); else { FCL_REAL s = C.length(); s = s1.radius / s; C *= s; } Vec3f c[3]; c[0] = T + dir_z * (0.5 * s1.lz); c[1] = T - dir_z * (0.5 * s1.lz) + C; c[2] = T - dir_z * (0.5 * s1.lz) - C; FCL_REAL d[3]; d[0] = new_s2.signedDistance(c[0]); d[1] = new_s2.signedDistance(c[1]); d[2] = new_s2.signedDistance(c[2]); if((d[0] >= 0 && d[1] >= 0 && d[2] >= 0) || (d[0] <= 0 && d[1] <= 0 && d[2] <= 0)) return false; else { bool positive[3]; for(std::size_t i = 0; i < 3; ++i) positive[i] = (d[i] >= 0); int n_positive = 0; FCL_REAL d_positive = 0, d_negative = 0; for(std::size_t i = 0; i < 3; ++i) { if(positive[i]) { n_positive++; if(d_positive <= d[i]) d_positive = d[i]; } else { if(d_negative <= -d[i]) d_negative = -d[i]; } } if (contacts) { const Vec3f normal = (d_positive > d_negative) ? -new_s2.n : new_s2.n; const FCL_REAL penetration_depth = std::min(d_positive, d_negative); Vec3f point; Vec3f p[2]; Vec3f q; FCL_REAL p_d[2]; FCL_REAL q_d(0); if(n_positive == 2) { for(std::size_t i = 0, j = 0; i < 3; ++i) { if(positive[i]) { p[j] = c[i]; p_d[j] = d[i]; j++; } else { q = c[i]; q_d = d[i]; } } const Vec3f t1 = (-p[0] * q_d + q * p_d[0]) / (-q_d + p_d[0]); const Vec3f t2 = (-p[1] * q_d + q * p_d[1]) / (-q_d + p_d[1]); point = (t1 + t2) * 0.5; } else { for(std::size_t i = 0, j = 0; i < 3; ++i) { if(!positive[i]) { p[j] = c[i]; p_d[j] = d[i]; j++; } else { q = c[i]; q_d = d[i]; } } const Vec3f t1 = (p[0] * q_d - q * p_d[0]) / (q_d - p_d[0]); const Vec3f t2 = (p[1] * q_d - q * p_d[1]) / (q_d - p_d[1]); point = (t1 + t2) * 0.5; } contacts->push_back(ContactPoint(normal, point, penetration_depth)); } return true; } } } bool convexPlaneIntersect(const Convex& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) { Plane new_s2 = transform(s2, tf2); Vec3f v_min, v_max; FCL_REAL d_min = std::numeric_limits::max(), d_max = -std::numeric_limits::max(); for(int i = 0; i < s1.num_points; ++i) { Vec3f p = tf1.transform(s1.points[i]); FCL_REAL d = new_s2.signedDistance(p); if(d < d_min) { d_min = d; v_min = p; } if(d > d_max) { d_max = d; v_max = p; } } if(d_min * d_max > 0) return false; else { if(d_min + d_max > 0) { if(penetration_depth) *penetration_depth = -d_min; if(normal) *normal = -new_s2.n; if(contact_points) *contact_points = v_min - new_s2.n * d_min; } else { if(penetration_depth) *penetration_depth = d_max; if(normal) *normal = new_s2.n; if(contact_points) *contact_points = v_max - new_s2.n * d_max; } return true; } } bool planeTriangleIntersect(const Plane& s1, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) { Plane new_s1 = transform(s1, tf1); Vec3f c[3]; c[0] = tf2.transform(P1); c[1] = tf2.transform(P2); c[2] = tf2.transform(P3); FCL_REAL d[3]; d[0] = new_s1.signedDistance(c[0]); d[1] = new_s1.signedDistance(c[1]); d[2] = new_s1.signedDistance(c[2]); if((d[0] >= 0 && d[1] >= 0 && d[2] >= 0) || (d[0] <= 0 && d[1] <= 0 && d[2] <= 0)) return false; else { bool positive[3]; for(std::size_t i = 0; i < 3; ++i) positive[i] = (d[i] > 0); int n_positive = 0; FCL_REAL d_positive = 0, d_negative = 0; for(std::size_t i = 0; i < 3; ++i) { if(positive[i]) { n_positive++; if(d_positive <= d[i]) d_positive = d[i]; } else { if(d_negative <= -d[i]) d_negative = -d[i]; } } if(penetration_depth) *penetration_depth = std::min(d_positive, d_negative); if(normal) *normal = (d_positive > d_negative) ? new_s1.n : -new_s1.n; if(contact_points) { Vec3f p[2]; Vec3f q; FCL_REAL p_d[2]; FCL_REAL q_d(0); if(n_positive == 2) { for(std::size_t i = 0, j = 0; i < 3; ++i) { if(positive[i]) { p[j] = c[i]; p_d[j] = d[i]; j++; } else { q = c[i]; q_d = d[i]; } } Vec3f t1 = (-p[0] * q_d + q * p_d[0]) / (-q_d + p_d[0]); Vec3f t2 = (-p[1] * q_d + q * p_d[1]) / (-q_d + p_d[1]); *contact_points = (t1 + t2) * 0.5; } else { for(std::size_t i = 0, j = 0; i < 3; ++i) { if(!positive[i]) { p[j] = c[i]; p_d[j] = d[i]; j++; } else { q = c[i]; q_d = d[i]; } } Vec3f t1 = (p[0] * q_d - q * p_d[0]) / (q_d - p_d[0]); Vec3f t2 = (p[1] * q_d - q * p_d[1]) / (q_d - p_d[1]); *contact_points = (t1 + t2) * 0.5; } } return true; } } bool halfspacePlaneIntersect(const Halfspace& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, Plane& pl, Vec3f& p, Vec3f& d, FCL_REAL& penetration_depth, int& ret) { return planeHalfspaceIntersect(s2, tf2, s1, tf1, pl, p, d, penetration_depth, ret); } bool planeIntersect(const Plane& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* /*contacts*/) { Plane new_s1 = transform(s1, tf1); Plane new_s2 = transform(s2, tf2); FCL_REAL a = (new_s1.n).dot(new_s2.n); if(a == 1 && new_s1.d != new_s2.d) return false; if(a == -1 && new_s1.d != -new_s2.d) return false; return true; } } // details // Shape intersect algorithms not using libccd // // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | box | O | | | | | | O | O | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | sphere |/////| O | | O | | | O | O | O | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | ellipsoid |/////|////////| | | | | O | O | TODO | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | capsule |/////|////////|///////////| | | | O | O | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | cone |/////|////////|///////////|/////////| | | O | O | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | cylinder |/////|////////|///////////|/////////|//////| | O | O | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | plane |/////|////////|///////////|/////////|//////|//////////| O | O | O | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | half-space |/////|////////|///////////|/////////|//////|//////////|///////| O | O | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ void flipNormal(std::vector& contacts) { for (std::vector::iterator it = contacts.begin(); it != contacts.end(); ++it) (*it).normal *= -1.0; } template<> bool GJKSolver_libccd::shapeIntersect(const Sphere &s1, const Transform3f& tf1, const Capsule &s2, const Transform3f& tf2, std::vector* contacts) const { return details::sphereCapsuleIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_libccd::shapeIntersect(const Capsule &s1, const Transform3f& tf1, const Sphere &s2, const Transform3f& tf2, std::vector* contacts) const { const bool res = details::sphereCapsuleIntersect(s2, tf2, s1, tf1, contacts); if (contacts) flipNormal(*contacts); return res; } template<> bool GJKSolver_libccd::shapeIntersect(const Sphere& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, std::vector* contacts) const { return details::sphereSphereIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_libccd::shapeIntersect(const Box& s1, const Transform3f& tf1, const Box& s2, const Transform3f& tf2, std::vector* contacts) const { return details::boxBoxIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_libccd::shapeIntersect(const Sphere& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) const { return details::sphereHalfspaceIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, std::vector* contacts) const { const bool res = details::sphereHalfspaceIntersect(s2, tf2, s1, tf1, contacts); if (contacts) flipNormal(*contacts); return res; } template<> bool GJKSolver_libccd::shapeIntersect(const Ellipsoid& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) const { return details::ellipsoidHalfspaceIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Ellipsoid& s2, const Transform3f& tf2, std::vector* contacts) const { const bool res = details::ellipsoidHalfspaceIntersect(s2, tf2, s1, tf1, contacts); if (contacts) flipNormal(*contacts); return res; } template<> bool GJKSolver_libccd::shapeIntersect(const Box& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) const { return details::boxHalfspaceIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Box& s2, const Transform3f& tf2, std::vector* contacts) const { const bool res = details::boxHalfspaceIntersect(s2, tf2, s1, tf1, contacts); if (contacts) flipNormal(*contacts); return res; } template<> bool GJKSolver_libccd::shapeIntersect(const Capsule& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) const { return details::capsuleHalfspaceIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Capsule& s2, const Transform3f& tf2, std::vector* contacts) const { const bool res = details::capsuleHalfspaceIntersect(s2, tf2, s1, tf1, contacts); if (contacts) flipNormal(*contacts); return res; } template<> bool GJKSolver_libccd::shapeIntersect(const Cylinder& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) const { return details::cylinderHalfspaceIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Cylinder& s2, const Transform3f& tf2, std::vector* contacts) const { const bool res = details::cylinderHalfspaceIntersect(s2, tf2, s1, tf1, contacts); if (contacts) flipNormal(*contacts); return res; } template<> bool GJKSolver_libccd::shapeIntersect(const Cone& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) const { return details::coneHalfspaceIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Cone& s2, const Transform3f& tf2, std::vector* contacts) const { const bool res = details::coneHalfspaceIntersect(s2, tf2, s1, tf1, contacts); if (contacts) flipNormal(*contacts); return res; } template<> bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) const { Halfspace s; Vec3f p, d; FCL_REAL depth; int ret; return details::halfspaceIntersect(s1, tf1, s2, tf2, p, d, s, depth, ret); } template<> bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) const { Plane pl; Vec3f p, d; FCL_REAL depth; int ret; return details::planeHalfspaceIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret); } template<> bool GJKSolver_libccd::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) const { Plane pl; Vec3f p, d; FCL_REAL depth; int ret; return details::halfspacePlaneIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret); } template<> bool GJKSolver_libccd::shapeIntersect(const Sphere& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) const { return details::spherePlaneIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, std::vector* contacts) const { const bool res = details::spherePlaneIntersect(s2, tf2, s1, tf1, contacts); if (contacts) flipNormal(*contacts); return res; } template<> bool GJKSolver_libccd::shapeIntersect(const Ellipsoid& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) const { return details::ellipsoidPlaneIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Ellipsoid& s2, const Transform3f& tf2, std::vector* contacts) const { const bool res = details::ellipsoidPlaneIntersect(s2, tf2, s1, tf1, contacts); if (contacts) flipNormal(*contacts); return res; } template<> bool GJKSolver_libccd::shapeIntersect(const Box& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) const { return details::boxPlaneIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Box& s2, const Transform3f& tf2, std::vector* contacts) const { const bool res = details::boxPlaneIntersect(s2, tf2, s1, tf1, contacts); if (contacts) flipNormal(*contacts); return res; } template<> bool GJKSolver_libccd::shapeIntersect(const Capsule& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) const { return details::capsulePlaneIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Capsule& s2, const Transform3f& tf2, std::vector* contacts) const { const bool res = details::capsulePlaneIntersect(s2, tf2, s1, tf1, contacts); if (contacts) flipNormal(*contacts); return res; } template<> bool GJKSolver_libccd::shapeIntersect(const Cylinder& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) const { return details::cylinderPlaneIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Cylinder& s2, const Transform3f& tf2, std::vector* contacts) const { const bool res = details::cylinderPlaneIntersect(s2, tf2, s1, tf1, contacts); if (contacts) flipNormal(*contacts); return res; } template<> bool GJKSolver_libccd::shapeIntersect(const Cone& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) const { return details::conePlaneIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Cone& s2, const Transform3f& tf2, std::vector* contacts) const { const bool res = details::conePlaneIntersect(s2, tf2, s1, tf1, contacts); if (contacts) flipNormal(*contacts); return res; } template<> bool GJKSolver_libccd::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) const { return details::planeIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const { return details::sphereTriangleIntersect(s, tf, P1, P2, P3, contact_points, penetration_depth, normal); } template<> bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const { return details::sphereTriangleIntersect(s, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P3), contact_points, penetration_depth, normal); } template<> bool GJKSolver_libccd::shapeTriangleIntersect(const Halfspace& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const { return details::halfspaceTriangleIntersect(s, tf1, P1, P2, P3, tf2, contact_points, penetration_depth, normal); } template<> bool GJKSolver_libccd::shapeTriangleIntersect(const Plane& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const { return details::planeTriangleIntersect(s, tf1, P1, P2, P3, tf2, contact_points, penetration_depth, normal); } // Shape distance algorithms not using libccd // // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | box | | | | | | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | sphere |/////| O | | O | | | | | O | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | ellipsoid |/////|////////| | | | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | capsule |/////|////////|///////////| O | | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | cone |/////|////////|///////////|/////////| | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | cylinder |/////|////////|///////////|/////////|//////| | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | plane |/////|////////|///////////|/////////|//////|//////////| | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | half-space |/////|////////|///////////|/////////|//////|//////////|///////| | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ template<> bool GJKSolver_libccd::shapeDistance(const Sphere& s1, const Transform3f& tf1, const Capsule& s2, const Transform3f& tf2, FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const { return details::sphereCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2); } template<> bool GJKSolver_libccd::shapeDistance(const Capsule& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const { return details::sphereCapsuleDistance(s2, tf2, s1, tf1, dist, p2, p1); } template<> bool GJKSolver_libccd::shapeDistance(const Sphere& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const { return details::sphereSphereDistance(s1, tf1, s2, tf2, dist, p1, p2); } template<> bool GJKSolver_libccd::shapeDistance(const Capsule& s1, const Transform3f& tf1, const Capsule& s2, const Transform3f& tf2, FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const { return details::capsuleCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2); } template<> bool GJKSolver_libccd::shapeTriangleDistance(const Sphere& s, const Transform3f& tf, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const { return details::sphereTriangleDistance(s, tf, P1, P2, P3, dist, p1, p2); } template<> bool GJKSolver_libccd::shapeTriangleDistance(const Sphere& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const { return details::sphereTriangleDistance(s, tf1, P1, P2, P3, tf2, dist, p1, p2); } // Shape intersect algorithms not using built-in GJK algorithm // // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | box | O | | | | | | O | O | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | sphere |/////| O | | O | | | O | O | O | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | ellipsoid |/////|////////| | | | | O | O | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | capsule |/////|////////|///////////| | | | O | O | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | cone |/////|////////|///////////|/////////| | | O | O | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | cylinder |/////|////////|///////////|/////////|//////| | O | O | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | plane |/////|////////|///////////|/////////|//////|//////////| O | O | O | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | half-space |/////|////////|///////////|/////////|//////|//////////|///////| O | O | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ template<> bool GJKSolver_indep::shapeIntersect(const Sphere &s1, const Transform3f& tf1, const Capsule &s2, const Transform3f& tf2, std::vector* contacts) const { return details::sphereCapsuleIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_indep::shapeIntersect(const Capsule &s1, const Transform3f& tf1, const Sphere &s2, const Transform3f& tf2, std::vector* contacts) const { const bool res = details::sphereCapsuleIntersect(s2, tf2, s1, tf1, contacts); if (contacts) flipNormal(*contacts); return res; } template<> bool GJKSolver_indep::shapeIntersect(const Sphere& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, std::vector* contacts) const { return details::sphereSphereIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_indep::shapeIntersect(const Box& s1, const Transform3f& tf1, const Box& s2, const Transform3f& tf2, std::vector* contacts) const { return details::boxBoxIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_indep::shapeIntersect(const Sphere& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) const { return details::sphereHalfspaceIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, std::vector* contacts) const { const bool res = details::sphereHalfspaceIntersect(s2, tf2, s1, tf1, contacts); if (contacts) flipNormal(*contacts); return res; } template<> bool GJKSolver_indep::shapeIntersect(const Ellipsoid& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) const { return details::ellipsoidHalfspaceIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Ellipsoid& s2, const Transform3f& tf2, std::vector* contacts) const { const bool res = details::ellipsoidHalfspaceIntersect(s2, tf2, s1, tf1, contacts); if (contacts) flipNormal(*contacts); return res; } template<> bool GJKSolver_indep::shapeIntersect(const Box& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) const { return details::boxHalfspaceIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Box& s2, const Transform3f& tf2, std::vector* contacts) const { const bool res = details::boxHalfspaceIntersect(s2, tf2, s1, tf1, contacts); if (contacts) flipNormal(*contacts); return res; } template<> bool GJKSolver_indep::shapeIntersect(const Capsule& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) const { return details::capsuleHalfspaceIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Capsule& s2, const Transform3f& tf2, std::vector* contacts) const { const bool res = details::capsuleHalfspaceIntersect(s2, tf2, s1, tf1, contacts); if (contacts) flipNormal(*contacts); return res; } template<> bool GJKSolver_indep::shapeIntersect(const Cylinder& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) const { return details::cylinderHalfspaceIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Cylinder& s2, const Transform3f& tf2, std::vector* contacts) const { const bool res = details::cylinderHalfspaceIntersect(s2, tf2, s1, tf1, contacts); if (contacts) flipNormal(*contacts); return res; } template<> bool GJKSolver_indep::shapeIntersect(const Cone& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) const { return details::coneHalfspaceIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Cone& s2, const Transform3f& tf2, std::vector* contacts) const { const bool res = details::coneHalfspaceIntersect(s2, tf2, s1, tf1, contacts); if (contacts) flipNormal(*contacts); return res; } template<> bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) const { Halfspace s; Vec3f p, d; FCL_REAL depth; int ret; return details::halfspaceIntersect(s1, tf1, s2, tf2, p, d, s, depth, ret); } template<> bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Halfspace& s2, const Transform3f& tf2, std::vector* contacts) const { Plane pl; Vec3f p, d; FCL_REAL depth; int ret; return details::planeHalfspaceIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret); } template<> bool GJKSolver_indep::shapeIntersect(const Halfspace& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) const { Plane pl; Vec3f p, d; FCL_REAL depth; int ret; return details::halfspacePlaneIntersect(s1, tf1, s2, tf2, pl, p, d, depth, ret); } template<> bool GJKSolver_indep::shapeIntersect(const Sphere& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) const { return details::spherePlaneIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, std::vector* contacts) const { const bool res = details::spherePlaneIntersect(s2, tf2, s1, tf1, contacts); if (contacts) flipNormal(*contacts); return res; } template<> bool GJKSolver_indep::shapeIntersect(const Ellipsoid& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) const { return details::ellipsoidPlaneIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Ellipsoid& s2, const Transform3f& tf2, std::vector* contacts) const { const bool res = details::ellipsoidPlaneIntersect(s2, tf2, s1, tf1, contacts); if (contacts) flipNormal(*contacts); return res; } template<> bool GJKSolver_indep::shapeIntersect(const Box& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) const { return details::boxPlaneIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Box& s2, const Transform3f& tf2, std::vector* contacts) const { const bool res = details::boxPlaneIntersect(s2, tf2, s1, tf1, contacts); if (contacts) flipNormal(*contacts); return res; } template<> bool GJKSolver_indep::shapeIntersect(const Capsule& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) const { return details::capsulePlaneIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Capsule& s2, const Transform3f& tf2, std::vector* contacts) const { const bool res = details::capsulePlaneIntersect(s2, tf2, s1, tf1, contacts); if (contacts) flipNormal(*contacts); return res; } template<> bool GJKSolver_indep::shapeIntersect(const Cylinder& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) const { return details::cylinderPlaneIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Cylinder& s2, const Transform3f& tf2, std::vector* contacts) const { const bool res = details::cylinderPlaneIntersect(s2, tf2, s1, tf1, contacts); if (contacts) flipNormal(*contacts); return res; } template<> bool GJKSolver_indep::shapeIntersect(const Cone& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) const { return details::conePlaneIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Cone& s2, const Transform3f& tf2, std::vector* contacts) const { const bool res = details::conePlaneIntersect(s2, tf2, s1, tf1, contacts); if (contacts) flipNormal(*contacts); return res; } template<> bool GJKSolver_indep::shapeIntersect(const Plane& s1, const Transform3f& tf1, const Plane& s2, const Transform3f& tf2, std::vector* contacts) const { return details::planeIntersect(s1, tf1, s2, tf2, contacts); } template<> bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const { return details::sphereTriangleIntersect(s, tf, P1, P2, P3, contact_points, penetration_depth, normal); } template<> bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const { return details::sphereTriangleIntersect(s, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P3), contact_points, penetration_depth, normal); } template<> bool GJKSolver_indep::shapeTriangleIntersect(const Halfspace& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const { return details::halfspaceTriangleIntersect(s, tf1, P1, P2, P3, tf2, contact_points, penetration_depth, normal); } template<> bool GJKSolver_indep::shapeTriangleIntersect(const Plane& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const { return details::planeTriangleIntersect(s, tf1, P1, P2, P3, tf2, contact_points, penetration_depth, normal); } // Shape distance algorithms not using built-in GJK algorithm // // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | box | | | | | | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | sphere |/////| O | | O | | | | | O | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | ellipsoid |/////|////////| | | | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | capsule |/////|////////|///////////| O | | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | cone |/////|////////|///////////|/////////| | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | cylinder |/////|////////|///////////|/////////|//////| | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | plane |/////|////////|///////////|/////////|//////|//////////| | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | half-space |/////|////////|///////////|/////////|//////|//////////|///////| | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ template<> bool GJKSolver_indep::shapeDistance(const Sphere& s1, const Transform3f& tf1, const Capsule& s2, const Transform3f& tf2, FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const { return details::sphereCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2); } template<> bool GJKSolver_indep::shapeDistance(const Capsule& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const { return details::sphereCapsuleDistance(s2, tf2, s1, tf1, dist, p2, p1); } template<> bool GJKSolver_indep::shapeDistance(const Sphere& s1, const Transform3f& tf1, const Sphere& s2, const Transform3f& tf2, FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const { return details::sphereSphereDistance(s1, tf1, s2, tf2, dist, p1, p2); } template<> bool GJKSolver_indep::shapeDistance(const Capsule& s1, const Transform3f& tf1, const Capsule& s2, const Transform3f& tf2, FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const { return details::capsuleCapsuleDistance(s1, tf1, s2, tf2, dist, p1, p2); } template<> bool GJKSolver_indep::shapeTriangleDistance(const Sphere& s, const Transform3f& tf, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const { return details::sphereTriangleDistance(s, tf, P1, P2, P3, dist, p1, p2); } template<> bool GJKSolver_indep::shapeTriangleDistance(const Sphere& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const { return details::sphereTriangleDistance(s, tf1, P1, P2, P3, tf2, dist, p1, p2); } } // fcl fcl-0.5.0/src/profile.cpp000066400000000000000000000171611274356571000152330ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2008-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Ioan Sucan */ #include "fcl/profile.h" #include fcl::tools::Profiler& fcl::tools::Profiler::Instance(void) { static Profiler p(true, false); return p; } #if ENABLE_PROFILING #include #include #include void fcl::tools::Profiler::start(void) { lock_.lock(); if (!running_) { tinfo_.set(); running_ = true; } lock_.unlock(); } void fcl::tools::Profiler::stop(void) { lock_.lock(); if (running_) { tinfo_.update(); running_ = false; } lock_.unlock(); } void fcl::tools::Profiler::clear(void) { lock_.lock(); data_.clear(); tinfo_ = TimeInfo(); if (running_) tinfo_.set(); lock_.unlock(); } void fcl::tools::Profiler::event(const std::string &name, const unsigned int times) { lock_.lock(); data_[std::this_thread::get_id()].events[name] += times; lock_.unlock(); } void fcl::tools::Profiler::average(const std::string &name, const double value) { lock_.lock(); AvgInfo &a = data_[std::this_thread::get_id()].avg[name]; a.total += value; a.totalSqr += value*value; a.parts++; lock_.unlock(); } void fcl::tools::Profiler::begin(const std::string &name) { lock_.lock(); data_[std::this_thread::get_id()].time[name].set(); lock_.unlock(); } void fcl::tools::Profiler::end(const std::string &name) { lock_.lock(); data_[std::this_thread::get_id()].time[name].update(); lock_.unlock(); } void fcl::tools::Profiler::status(std::ostream &out, bool merge) { stop(); lock_.lock(); printOnDestroy_ = false; out << std::endl; out << " *** Profiling statistics. Total counted time : " << time::seconds(tinfo_.total) << " seconds" << std::endl; if (merge) { PerThread combined; for (std::map::const_iterator it = data_.begin() ; it != data_.end() ; ++it) { for (std::map::const_iterator iev = it->second.events.begin() ; iev != it->second.events.end(); ++iev) combined.events[iev->first] += iev->second; for (std::map::const_iterator iavg = it->second.avg.begin() ; iavg != it->second.avg.end(); ++iavg) { combined.avg[iavg->first].total += iavg->second.total; combined.avg[iavg->first].totalSqr += iavg->second.totalSqr; combined.avg[iavg->first].parts += iavg->second.parts; } for (std::map::const_iterator itm = it->second.time.begin() ; itm != it->second.time.end(); ++itm) { TimeInfo &tc = combined.time[itm->first]; tc.total = tc.total + itm->second.total; tc.parts = tc.parts + itm->second.parts; if (tc.shortest > itm->second.shortest) tc.shortest = itm->second.shortest; if (tc.longest < itm->second.longest) tc.longest = itm->second.longest; } } printThreadInfo(out, combined); } else for (std::map::const_iterator it = data_.begin() ; it != data_.end() ; ++it) { out << "Thread " << it->first << ":" << std::endl; printThreadInfo(out, it->second); } lock_.unlock(); } /// @cond IGNORE namespace fcl { struct dataIntVal { std::string name; unsigned long int value; }; struct SortIntByValue { bool operator()(const dataIntVal &a, const dataIntVal &b) const { return a.value > b.value; } }; struct dataDoubleVal { std::string name; double value; }; struct SortDoubleByValue { bool operator()(const dataDoubleVal &a, const dataDoubleVal &b) const { return a.value > b.value; } }; } /// @endcond void fcl::tools::Profiler::printThreadInfo(std::ostream &out, const PerThread &data) { double total = time::seconds(tinfo_.total); std::vector events; for (std::map::const_iterator iev = data.events.begin() ; iev != data.events.end() ; ++iev) { dataIntVal next = {iev->first, iev->second}; events.push_back(next); } std::sort(events.begin(), events.end(), SortIntByValue()); if (!events.empty()) out << "Events:" << std::endl; for (unsigned int i = 0 ; i < events.size() ; ++i) out << events[i].name << ": " << events[i].value << std::endl; std::vector avg; for (std::map::const_iterator ia = data.avg.begin() ; ia != data.avg.end() ; ++ia) { dataDoubleVal next = {ia->first, ia->second.total / (double)ia->second.parts}; avg.push_back(next); } std::sort(avg.begin(), avg.end(), SortDoubleByValue()); if (!avg.empty()) out << "Averages:" << std::endl; for (unsigned int i = 0 ; i < avg.size() ; ++i) { const AvgInfo &a = data.avg.find(avg[i].name)->second; out << avg[i].name << ": " << avg[i].value << " (stddev = " << std::sqrt(std::abs(a.totalSqr - (double)a.parts * avg[i].value * avg[i].value) / ((double)a.parts - 1.)) << ")" << std::endl; } std::vector time; for (std::map::const_iterator itm = data.time.begin() ; itm != data.time.end() ; ++itm) { dataDoubleVal next = {itm->first, time::seconds(itm->second.total)}; time.push_back(next); } std::sort(time.begin(), time.end(), SortDoubleByValue()); if (!time.empty()) out << "Blocks of time:" << std::endl; double unaccounted = total; for (unsigned int i = 0 ; i < time.size() ; ++i) { const TimeInfo &d = data.time.find(time[i].name)->second; double tS = time::seconds(d.shortest); double tL = time::seconds(d.longest); out << time[i].name << ": " << time[i].value << "s (" << (100.0 * time[i].value/total) << "%), [" << tS << "s --> " << tL << " s], " << d.parts << " parts"; if (d.parts > 0) out << ", " << (time::seconds(d.total) / (double)d.parts) << " s on average"; out << std::endl; unaccounted -= time[i].value; } out << "Unaccounted time : " << unaccounted; if (total > 0.0) out << " (" << (100.0 * unaccounted / total) << " %)"; out << std::endl; out << std::endl; } #endif fcl-0.5.0/src/shape/000077500000000000000000000000001274356571000141615ustar00rootroot00000000000000fcl-0.5.0/src/shape/geometric_shapes.cpp000066400000000000000000000123231274356571000202070ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/shape/geometric_shapes.h" #include "fcl/shape/geometric_shapes_utility.h" namespace fcl { void Convex::fillEdges() { int* points_in_poly = polygons; if(edges) delete [] edges; int num_edges_alloc = 0; for(int i = 0; i < num_planes; ++i) { num_edges_alloc += *points_in_poly; points_in_poly += (*points_in_poly + 1); } edges = new Edge[num_edges_alloc]; points_in_poly = polygons; int* index = polygons + 1; num_edges = 0; Edge e; bool isinset; for(int i = 0; i < num_planes; ++i) { for(int j = 0; j < *points_in_poly; ++j) { e.first = std::min(index[j], index[(j+1)%*points_in_poly]); e.second = std::max(index[j], index[(j+1)%*points_in_poly]); isinset = false; for(int k = 0; k < num_edges; ++k) { if((edges[k].first == e.first) && (edges[k].second == e.second)) { isinset = true; break; } } if(!isinset) { edges[num_edges].first = e.first; edges[num_edges].second = e.second; ++num_edges; } } points_in_poly += (*points_in_poly + 1); index = points_in_poly + 1; } if(num_edges < num_edges_alloc) { Edge* tmp = new Edge[num_edges]; memcpy(tmp, edges, num_edges * sizeof(Edge)); delete [] edges; edges = tmp; } } void Halfspace::unitNormalTest() { FCL_REAL l = n.length(); if(l > 0) { FCL_REAL inv_l = 1.0 / l; n *= inv_l; d *= inv_l; } else { n.setValue(1, 0, 0); d = 0; } } void Plane::unitNormalTest() { FCL_REAL l = n.length(); if(l > 0) { FCL_REAL inv_l = 1.0 / l; n *= inv_l; d *= inv_l; } else { n.setValue(1, 0, 0); d = 0; } } void Box::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).length(); } void Sphere::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); aabb_center = aabb_local.center(); aabb_radius = radius; } void Ellipsoid::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).length(); } void Capsule::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).length(); } void Cone::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).length(); } void Cylinder::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).length(); } void Convex::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).length(); } void Halfspace::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).length(); } void Plane::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).length(); } void TriangleP::computeLocalAABB() { computeBV(*this, Transform3f(), aabb_local); aabb_center = aabb_local.center(); aabb_radius = (aabb_local.min_ - aabb_center).length(); } } fcl-0.5.0/src/shape/geometric_shapes_utility.cpp000066400000000000000000001025641274356571000220010ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/shape/geometric_shapes_utility.h" #include "fcl/BVH/BV_fitter.h" namespace fcl { namespace details { std::vector getBoundVertices(const Box& box, const Transform3f& tf) { std::vector result(8); FCL_REAL a = box.side[0] / 2; FCL_REAL b = box.side[1] / 2; FCL_REAL c = box.side[2] / 2; result[0] = tf.transform(Vec3f(a, b, c)); result[1] = tf.transform(Vec3f(a, b, -c)); result[2] = tf.transform(Vec3f(a, -b, c)); result[3] = tf.transform(Vec3f(a, -b, -c)); result[4] = tf.transform(Vec3f(-a, b, c)); result[5] = tf.transform(Vec3f(-a, b, -c)); result[6] = tf.transform(Vec3f(-a, -b, c)); result[7] = tf.transform(Vec3f(-a, -b, -c)); return result; } // we use icosahedron to bound the sphere std::vector getBoundVertices(const Sphere& sphere, const Transform3f& tf) { std::vector result(12); const FCL_REAL m = (1 + sqrt(5.0)) / 2.0; FCL_REAL edge_size = sphere.radius * 6 / (sqrt(27.0) + sqrt(15.0)); FCL_REAL a = edge_size; FCL_REAL b = m * edge_size; result[0] = tf.transform(Vec3f(0, a, b)); result[1] = tf.transform(Vec3f(0, -a, b)); result[2] = tf.transform(Vec3f(0, a, -b)); result[3] = tf.transform(Vec3f(0, -a, -b)); result[4] = tf.transform(Vec3f(a, b, 0)); result[5] = tf.transform(Vec3f(-a, b, 0)); result[6] = tf.transform(Vec3f(a, -b, 0)); result[7] = tf.transform(Vec3f(-a, -b, 0)); result[8] = tf.transform(Vec3f(b, 0, a)); result[9] = tf.transform(Vec3f(b, 0, -a)); result[10] = tf.transform(Vec3f(-b, 0, a)); result[11] = tf.transform(Vec3f(-b, 0, -a)); return result; } std::vector getBoundVertices(const Ellipsoid& ellipsoid, const Transform3f& tf) { // we use scaled icosahedron to bound the ellipsoid std::vector result(12); const FCL_REAL phi = (1.0 + std::sqrt(5.0)) / 2.0; // golden ratio const FCL_REAL a = std::sqrt(3.0) / (phi * phi); const FCL_REAL b = phi * a; const FCL_REAL& A = ellipsoid.radii[0]; const FCL_REAL& B = ellipsoid.radii[1]; const FCL_REAL& C = ellipsoid.radii[2]; const FCL_REAL Aa = A * a; const FCL_REAL Ab = A * b; const FCL_REAL Ba = B * a; const FCL_REAL Bb = B * b; const FCL_REAL Ca = C * a; const FCL_REAL Cb = C * b; result[0] = tf.transform(Vec3f(0, Ba, Cb)); result[1] = tf.transform(Vec3f(0, -Ba, Cb)); result[2] = tf.transform(Vec3f(0, Ba, -Cb)); result[3] = tf.transform(Vec3f(0, -Ba, -Cb)); result[4] = tf.transform(Vec3f(Aa, Bb, 0)); result[5] = tf.transform(Vec3f(-Aa, Bb, 0)); result[6] = tf.transform(Vec3f(Aa, -Bb, 0)); result[7] = tf.transform(Vec3f(-Aa, -Bb, 0)); result[8] = tf.transform(Vec3f(Ab, 0, Ca)); result[9] = tf.transform(Vec3f(Ab, 0, -Ca)); result[10] = tf.transform(Vec3f(-Ab, 0, Ca)); result[11] = tf.transform(Vec3f(-Ab, 0, -Ca)); return result; } std::vector getBoundVertices(const Capsule& capsule, const Transform3f& tf) { std::vector result(36); const FCL_REAL m = (1 + sqrt(5.0)) / 2.0; FCL_REAL hl = capsule.lz * 0.5; FCL_REAL edge_size = capsule.radius * 6 / (sqrt(27.0) + sqrt(15.0)); FCL_REAL a = edge_size; FCL_REAL b = m * edge_size; FCL_REAL r2 = capsule.radius * 2 / sqrt(3.0); result[0] = tf.transform(Vec3f(0, a, b + hl)); result[1] = tf.transform(Vec3f(0, -a, b + hl)); result[2] = tf.transform(Vec3f(0, a, -b + hl)); result[3] = tf.transform(Vec3f(0, -a, -b + hl)); result[4] = tf.transform(Vec3f(a, b, hl)); result[5] = tf.transform(Vec3f(-a, b, hl)); result[6] = tf.transform(Vec3f(a, -b, hl)); result[7] = tf.transform(Vec3f(-a, -b, hl)); result[8] = tf.transform(Vec3f(b, 0, a + hl)); result[9] = tf.transform(Vec3f(b, 0, -a + hl)); result[10] = tf.transform(Vec3f(-b, 0, a + hl)); result[11] = tf.transform(Vec3f(-b, 0, -a + hl)); result[12] = tf.transform(Vec3f(0, a, b - hl)); result[13] = tf.transform(Vec3f(0, -a, b - hl)); result[14] = tf.transform(Vec3f(0, a, -b - hl)); result[15] = tf.transform(Vec3f(0, -a, -b - hl)); result[16] = tf.transform(Vec3f(a, b, -hl)); result[17] = tf.transform(Vec3f(-a, b, -hl)); result[18] = tf.transform(Vec3f(a, -b, -hl)); result[19] = tf.transform(Vec3f(-a, -b, -hl)); result[20] = tf.transform(Vec3f(b, 0, a - hl)); result[21] = tf.transform(Vec3f(b, 0, -a - hl)); result[22] = tf.transform(Vec3f(-b, 0, a - hl)); result[23] = tf.transform(Vec3f(-b, 0, -a - hl)); FCL_REAL c = 0.5 * r2; FCL_REAL d = capsule.radius; result[24] = tf.transform(Vec3f(r2, 0, hl)); result[25] = tf.transform(Vec3f(c, d, hl)); result[26] = tf.transform(Vec3f(-c, d, hl)); result[27] = tf.transform(Vec3f(-r2, 0, hl)); result[28] = tf.transform(Vec3f(-c, -d, hl)); result[29] = tf.transform(Vec3f(c, -d, hl)); result[30] = tf.transform(Vec3f(r2, 0, -hl)); result[31] = tf.transform(Vec3f(c, d, -hl)); result[32] = tf.transform(Vec3f(-c, d, -hl)); result[33] = tf.transform(Vec3f(-r2, 0, -hl)); result[34] = tf.transform(Vec3f(-c, -d, -hl)); result[35] = tf.transform(Vec3f(c, -d, -hl)); return result; } std::vector getBoundVertices(const Cone& cone, const Transform3f& tf) { std::vector result(7); FCL_REAL hl = cone.lz * 0.5; FCL_REAL r2 = cone.radius * 2 / sqrt(3.0); FCL_REAL a = 0.5 * r2; FCL_REAL b = cone.radius; result[0] = tf.transform(Vec3f(r2, 0, -hl)); result[1] = tf.transform(Vec3f(a, b, -hl)); result[2] = tf.transform(Vec3f(-a, b, -hl)); result[3] = tf.transform(Vec3f(-r2, 0, -hl)); result[4] = tf.transform(Vec3f(-a, -b, -hl)); result[5] = tf.transform(Vec3f(a, -b, -hl)); result[6] = tf.transform(Vec3f(0, 0, hl)); return result; } std::vector getBoundVertices(const Cylinder& cylinder, const Transform3f& tf) { std::vector result(12); FCL_REAL hl = cylinder.lz * 0.5; FCL_REAL r2 = cylinder.radius * 2 / sqrt(3.0); FCL_REAL a = 0.5 * r2; FCL_REAL b = cylinder.radius; result[0] = tf.transform(Vec3f(r2, 0, -hl)); result[1] = tf.transform(Vec3f(a, b, -hl)); result[2] = tf.transform(Vec3f(-a, b, -hl)); result[3] = tf.transform(Vec3f(-r2, 0, -hl)); result[4] = tf.transform(Vec3f(-a, -b, -hl)); result[5] = tf.transform(Vec3f(a, -b, -hl)); result[6] = tf.transform(Vec3f(r2, 0, hl)); result[7] = tf.transform(Vec3f(a, b, hl)); result[8] = tf.transform(Vec3f(-a, b, hl)); result[9] = tf.transform(Vec3f(-r2, 0, hl)); result[10] = tf.transform(Vec3f(-a, -b, hl)); result[11] = tf.transform(Vec3f(a, -b, hl)); return result; } std::vector getBoundVertices(const Convex& convex, const Transform3f& tf) { std::vector result(convex.num_points); for(int i = 0; i < convex.num_points; ++i) { result[i] = tf.transform(convex.points[i]); } return result; } std::vector getBoundVertices(const TriangleP& triangle, const Transform3f& tf) { std::vector result(3); result[0] = tf.transform(triangle.a); result[1] = tf.transform(triangle.b); result[2] = tf.transform(triangle.c); return result; } } // end detail Halfspace transform(const Halfspace& a, const Transform3f& tf) { /// suppose the initial halfspace is n * x <= d /// after transform (R, T), x --> x' = R x + T /// and the new half space becomes n' * x' <= d' /// where n' = R * n /// and d' = d + n' * T Vec3f n = tf.getQuatRotation().transform(a.n); FCL_REAL d = a.d + n.dot(tf.getTranslation()); return Halfspace(n, d); } Plane transform(const Plane& a, const Transform3f& tf) { /// suppose the initial halfspace is n * x <= d /// after transform (R, T), x --> x' = R x + T /// and the new half space becomes n' * x' <= d' /// where n' = R * n /// and d' = d + n' * T Vec3f n = tf.getQuatRotation().transform(a.n); FCL_REAL d = a.d + n.dot(tf.getTranslation()); return Plane(n, d); } template<> void computeBV(const Box& s, const Transform3f& tf, AABB& bv) { const Matrix3f& R = tf.getRotation(); const Vec3f& T = tf.getTranslation(); FCL_REAL x_range = 0.5 * (fabs(R(0, 0) * s.side[0]) + fabs(R(0, 1) * s.side[1]) + fabs(R(0, 2) * s.side[2])); FCL_REAL y_range = 0.5 * (fabs(R(1, 0) * s.side[0]) + fabs(R(1, 1) * s.side[1]) + fabs(R(1, 2) * s.side[2])); FCL_REAL z_range = 0.5 * (fabs(R(2, 0) * s.side[0]) + fabs(R(2, 1) * s.side[1]) + fabs(R(2, 2) * s.side[2])); Vec3f v_delta(x_range, y_range, z_range); bv.max_ = T + v_delta; bv.min_ = T - v_delta; } template<> void computeBV(const Sphere& s, const Transform3f& tf, AABB& bv) { const Vec3f& T = tf.getTranslation(); Vec3f v_delta(s.radius); bv.max_ = T + v_delta; bv.min_ = T - v_delta; } template<> void computeBV(const Ellipsoid& s, const Transform3f& tf, AABB& bv) { const Matrix3f& R = tf.getRotation(); const Vec3f& T = tf.getTranslation(); FCL_REAL x_range = (fabs(R(0, 0) * s.radii[0]) + fabs(R(0, 1) * s.radii[1]) + fabs(R(0, 2) * s.radii[2])); FCL_REAL y_range = (fabs(R(1, 0) * s.radii[0]) + fabs(R(1, 1) * s.radii[1]) + fabs(R(1, 2) * s.radii[2])); FCL_REAL z_range = (fabs(R(2, 0) * s.radii[0]) + fabs(R(2, 1) * s.radii[1]) + fabs(R(2, 2) * s.radii[2])); Vec3f v_delta(x_range, y_range, z_range); bv.max_ = T + v_delta; bv.min_ = T - v_delta; } template<> void computeBV(const Capsule& s, const Transform3f& tf, AABB& bv) { const Matrix3f& R = tf.getRotation(); const Vec3f& T = tf.getTranslation(); FCL_REAL x_range = 0.5 * fabs(R(0, 2) * s.lz) + s.radius; FCL_REAL y_range = 0.5 * fabs(R(1, 2) * s.lz) + s.radius; FCL_REAL z_range = 0.5 * fabs(R(2, 2) * s.lz) + s.radius; Vec3f v_delta(x_range, y_range, z_range); bv.max_ = T + v_delta; bv.min_ = T - v_delta; } template<> void computeBV(const Cone& s, const Transform3f& tf, AABB& bv) { const Matrix3f& R = tf.getRotation(); const Vec3f& T = tf.getTranslation(); FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + 0.5 * fabs(R(0, 2) * s.lz); FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + 0.5 * fabs(R(1, 2) * s.lz); FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + 0.5 * fabs(R(2, 2) * s.lz); Vec3f v_delta(x_range, y_range, z_range); bv.max_ = T + v_delta; bv.min_ = T - v_delta; } template<> void computeBV(const Cylinder& s, const Transform3f& tf, AABB& bv) { const Matrix3f& R = tf.getRotation(); const Vec3f& T = tf.getTranslation(); FCL_REAL x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + 0.5 * fabs(R(0, 2) * s.lz); FCL_REAL y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + 0.5 * fabs(R(1, 2) * s.lz); FCL_REAL z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + 0.5 * fabs(R(2, 2) * s.lz); Vec3f v_delta(x_range, y_range, z_range); bv.max_ = T + v_delta; bv.min_ = T - v_delta; } template<> void computeBV(const Convex& s, const Transform3f& tf, AABB& bv) { const Matrix3f& R = tf.getRotation(); const Vec3f& T = tf.getTranslation(); AABB bv_; for(int i = 0; i < s.num_points; ++i) { Vec3f new_p = R * s.points[i] + T; bv_ += new_p; } bv = bv_; } template<> void computeBV(const TriangleP& s, const Transform3f& tf, AABB& bv) { bv = AABB(tf.transform(s.a), tf.transform(s.b), tf.transform(s.c)); } template<> void computeBV(const Halfspace& s, const Transform3f& tf, AABB& bv) { Halfspace new_s = transform(s, tf); const Vec3f& n = new_s.n; const FCL_REAL& d = new_s.d; AABB bv_; bv_.min_ = Vec3f(-std::numeric_limits::max()); bv_.max_ = Vec3f(std::numeric_limits::max()); if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { // normal aligned with x axis if(n[0] < 0) bv_.min_[0] = -d; else if(n[0] > 0) bv_.max_[0] = d; } else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { // normal aligned with y axis if(n[1] < 0) bv_.min_[1] = -d; else if(n[1] > 0) bv_.max_[1] = d; } else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { // normal aligned with z axis if(n[2] < 0) bv_.min_[2] = -d; else if(n[2] > 0) bv_.max_[2] = d; } bv = bv_; } template<> void computeBV(const Plane& s, const Transform3f& tf, AABB& bv) { Plane new_s = transform(s, tf); const Vec3f& n = new_s.n; const FCL_REAL& d = new_s.d; AABB bv_; bv_.min_ = Vec3f(-std::numeric_limits::max()); bv_.max_ = Vec3f(std::numeric_limits::max()); if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { // normal aligned with x axis if(n[0] < 0) { bv_.min_[0] = bv_.max_[0] = -d; } else if(n[0] > 0) { bv_.min_[0] = bv_.max_[0] = d; } } else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { // normal aligned with y axis if(n[1] < 0) { bv_.min_[1] = bv_.max_[1] = -d; } else if(n[1] > 0) { bv_.min_[1] = bv_.max_[1] = d; } } else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { // normal aligned with z axis if(n[2] < 0) { bv_.min_[2] = bv_.max_[2] = -d; } else if(n[2] > 0) { bv_.min_[2] = bv_.max_[2] = d; } } bv = bv_; } template<> void computeBV(const Box& s, const Transform3f& tf, OBB& bv) { const Matrix3f& R = tf.getRotation(); const Vec3f& T = tf.getTranslation(); bv.To = T; bv.axis[0] = R.getColumn(0); bv.axis[1] = R.getColumn(1); bv.axis[2] = R.getColumn(2); bv.extent = s.side * (FCL_REAL)0.5; } template<> void computeBV(const Sphere& s, const Transform3f& tf, OBB& bv) { const Vec3f& T = tf.getTranslation(); bv.To = T; bv.axis[0].setValue(1, 0, 0); bv.axis[1].setValue(0, 1, 0); bv.axis[2].setValue(0, 0, 1); bv.extent.setValue(s.radius); } template<> void computeBV(const Ellipsoid& s, const Transform3f& tf, OBB& bv) { const Matrix3f& R = tf.getRotation(); const Vec3f& T = tf.getTranslation(); bv.To = T; bv.axis[0] = R.getColumn(0); bv.axis[1] = R.getColumn(1); bv.axis[2] = R.getColumn(2); bv.extent = s.radii; } template<> void computeBV(const Capsule& s, const Transform3f& tf, OBB& bv) { const Matrix3f& R = tf.getRotation(); const Vec3f& T = tf.getTranslation(); bv.To = T; bv.axis[0] = R.getColumn(0); bv.axis[1] = R.getColumn(1); bv.axis[2] = R.getColumn(2); bv.extent.setValue(s.radius, s.radius, s.lz / 2 + s.radius); } template<> void computeBV(const Cone& s, const Transform3f& tf, OBB& bv) { const Matrix3f& R = tf.getRotation(); const Vec3f& T = tf.getTranslation(); bv.To = T; bv.axis[0] = R.getColumn(0); bv.axis[1] = R.getColumn(1); bv.axis[2] = R.getColumn(2); bv.extent.setValue(s.radius, s.radius, s.lz / 2); } template<> void computeBV(const Cylinder& s, const Transform3f& tf, OBB& bv) { const Matrix3f& R = tf.getRotation(); const Vec3f& T = tf.getTranslation(); bv.To = T; bv.axis[0] = R.getColumn(0); bv.axis[1] = R.getColumn(1); bv.axis[2] = R.getColumn(2); bv.extent.setValue(s.radius, s.radius, s.lz / 2); } template<> void computeBV(const Convex& s, const Transform3f& tf, OBB& bv) { const Matrix3f& R = tf.getRotation(); const Vec3f& T = tf.getTranslation(); fit(s.points, s.num_points, bv); bv.axis[0] = R * bv.axis[0]; bv.axis[1] = R * bv.axis[1]; bv.axis[2] = R * bv.axis[2]; bv.To = R * bv.To + T; } template<> void computeBV(const Halfspace& s, const Transform3f& tf, OBB& bv) { /// Half space can only have very rough OBB bv.axis[0] = Vec3f(1, 0, 0); bv.axis[1] = Vec3f(0, 1, 0); bv.axis[2] = Vec3f(0, 0, 1); bv.To = Vec3f(0, 0, 0); bv.extent.setValue(std::numeric_limits::max()); } template<> void computeBV(const Halfspace& s, const Transform3f& tf, RSS& bv) { /// Half space can only have very rough RSS bv.axis[0] = Vec3f(1, 0, 0); bv.axis[1] = Vec3f(0, 1, 0); bv.axis[2] = Vec3f(0, 0, 1); bv.Tr = Vec3f(0, 0, 0); bv.l[0] = bv.l[1] = bv.r = std::numeric_limits::max(); } template<> void computeBV(const Halfspace& s, const Transform3f& tf, OBBRSS& bv) { computeBV(s, tf, bv.obb); computeBV(s, tf, bv.rss); } template<> void computeBV(const Halfspace& s, const Transform3f& tf, kIOS& bv) { bv.num_spheres = 1; computeBV(s, tf, bv.obb); bv.spheres[0].o = Vec3f(); bv.spheres[0].r = std::numeric_limits::max(); } template<> void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<16>& bv) { Halfspace new_s = transform(s, tf); const Vec3f& n = new_s.n; const FCL_REAL& d = new_s.d; const std::size_t D = 8; for(std::size_t i = 0; i < D; ++i) bv.dist(i) = -std::numeric_limits::max(); for(std::size_t i = D; i < 2 * D; ++i) bv.dist(i) = std::numeric_limits::max(); if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { if(n[0] > 0) bv.dist(D) = d; else bv.dist(0) = -d; } else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { if(n[1] > 0) bv.dist(D + 1) = d; else bv.dist(1) = -d; } else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { if(n[2] > 0) bv.dist(D + 2) = d; else bv.dist(2) = -d; } else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1]) { if(n[0] > 0) bv.dist(D + 3) = n[0] * d * 2; else bv.dist(3) = n[0] * d * 2; } else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2]) { if(n[1] > 0) bv.dist(D + 4) = n[0] * d * 2; else bv.dist(4) = n[0] * d * 2; } else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2]) { if(n[1] > 0) bv.dist(D + 5) = n[1] * d * 2; else bv.dist(5) = n[1] * d * 2; } else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { if(n[0] > 0) bv.dist(D + 6) = n[0] * d * 2; else bv.dist(6) = n[0] * d * 2; } else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { if(n[0] > 0) bv.dist(D + 7) = n[0] * d * 2; else bv.dist(7) = n[0] * d * 2; } } template<> void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<18>& bv) { Halfspace new_s = transform(s, tf); const Vec3f& n = new_s.n; const FCL_REAL& d = new_s.d; const std::size_t D = 9; for(std::size_t i = 0; i < D; ++i) bv.dist(i) = -std::numeric_limits::max(); for(std::size_t i = D; i < 2 * D; ++i) bv.dist(i) = std::numeric_limits::max(); if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { if(n[0] > 0) bv.dist(D) = d; else bv.dist(0) = -d; } else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { if(n[1] > 0) bv.dist(D + 1) = d; else bv.dist(1) = -d; } else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { if(n[2] > 0) bv.dist(D + 2) = d; else bv.dist(2) = -d; } else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1]) { if(n[0] > 0) bv.dist(D + 3) = n[0] * d * 2; else bv.dist(3) = n[0] * d * 2; } else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2]) { if(n[1] > 0) bv.dist(D + 4) = n[0] * d * 2; else bv.dist(4) = n[0] * d * 2; } else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2]) { if(n[1] > 0) bv.dist(D + 5) = n[1] * d * 2; else bv.dist(5) = n[1] * d * 2; } else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { if(n[0] > 0) bv.dist(D + 6) = n[0] * d * 2; else bv.dist(6) = n[0] * d * 2; } else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { if(n[0] > 0) bv.dist(D + 7) = n[0] * d * 2; else bv.dist(7) = n[0] * d * 2; } else if(n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) { if(n[1] > 0) bv.dist(D + 8) = n[1] * d * 2; else bv.dist(8) = n[1] * d * 2; } } template<> void computeBV, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<24>& bv) { Halfspace new_s = transform(s, tf); const Vec3f& n = new_s.n; const FCL_REAL& d = new_s.d; const std::size_t D = 12; for(std::size_t i = 0; i < D; ++i) bv.dist(i) = -std::numeric_limits::max(); for(std::size_t i = D; i < 2 * D; ++i) bv.dist(i) = std::numeric_limits::max(); if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { if(n[0] > 0) bv.dist(D) = d; else bv.dist(0) = -d; } else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { if(n[1] > 0) bv.dist(D + 1) = d; else bv.dist(1) = -d; } else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { if(n[2] > 0) bv.dist(D + 2) = d; else bv.dist(2) = -d; } else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1]) { if(n[0] > 0) bv.dist(D + 3) = n[0] * d * 2; else bv.dist(3) = n[0] * d * 2; } else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2]) { if(n[1] > 0) bv.dist(D + 4) = n[0] * d * 2; else bv.dist(4) = n[0] * d * 2; } else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2]) { if(n[1] > 0) bv.dist(D + 5) = n[1] * d * 2; else bv.dist(5) = n[1] * d * 2; } else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { if(n[0] > 0) bv.dist(D + 6) = n[0] * d * 2; else bv.dist(6) = n[0] * d * 2; } else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { if(n[0] > 0) bv.dist(D + 7) = n[0] * d * 2; else bv.dist(7) = n[0] * d * 2; } else if(n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) { if(n[1] > 0) bv.dist(D + 8) = n[1] * d * 2; else bv.dist(8) = n[1] * d * 2; } else if(n[0] + n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { if(n[0] > 0) bv.dist(D + 9) = n[0] * d * 3; else bv.dist(9) = n[0] * d * 3; } else if(n[0] + n[1] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) { if(n[0] > 0) bv.dist(D + 10) = n[0] * d * 3; else bv.dist(10) = n[0] * d * 3; } else if(n[0] + n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { if(n[1] > 0) bv.dist(D + 11) = n[1] * d * 3; else bv.dist(11) = n[1] * d * 3; } } template<> void computeBV(const Plane& s, const Transform3f& tf, OBB& bv) { Vec3f n = tf.getQuatRotation().transform(s.n); generateCoordinateSystem(n, bv.axis[1], bv.axis[2]); bv.axis[0] = n; bv.extent.setValue(0, std::numeric_limits::max(), std::numeric_limits::max()); Vec3f p = s.n * s.d; bv.To = tf.transform(p); /// n'd' = R * n * (d + (R * n) * T) = R * (n * d) + T } template<> void computeBV(const Plane& s, const Transform3f& tf, RSS& bv) { Vec3f n = tf.getQuatRotation().transform(s.n); generateCoordinateSystem(n, bv.axis[1], bv.axis[2]); bv.axis[0] = n; bv.l[0] = std::numeric_limits::max(); bv.l[1] = std::numeric_limits::max(); bv.r = 0; Vec3f p = s.n * s.d; bv.Tr = tf.transform(p); } template<> void computeBV(const Plane& s, const Transform3f& tf, OBBRSS& bv) { computeBV(s, tf, bv.obb); computeBV(s, tf, bv.rss); } template<> void computeBV(const Plane& s, const Transform3f& tf, kIOS& bv) { bv.num_spheres = 1; computeBV(s, tf, bv.obb); bv.spheres[0].o = Vec3f(); bv.spheres[0].r = std::numeric_limits::max(); } template<> void computeBV, Plane>(const Plane& s, const Transform3f& tf, KDOP<16>& bv) { Plane new_s = transform(s, tf); const Vec3f& n = new_s.n; const FCL_REAL& d = new_s.d; const std::size_t D = 8; for(std::size_t i = 0; i < D; ++i) bv.dist(i) = -std::numeric_limits::max(); for(std::size_t i = D; i < 2 * D; ++i) bv.dist(i) = std::numeric_limits::max(); if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { if(n[0] > 0) bv.dist(0) = bv.dist(D) = d; else bv.dist(0) = bv.dist(D) = -d; } else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { if(n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d; else bv.dist(1) = bv.dist(D + 1) = -d; } else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { if(n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d; else bv.dist(2) = bv.dist(D + 2) = -d; } else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1]) { bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2; } else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2]) { bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2; } else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2]) { bv.dist(6) = bv.dist(D + 5) = n[1] * d * 2; } else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2; } else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2; } } template<> void computeBV, Plane>(const Plane& s, const Transform3f& tf, KDOP<18>& bv) { Plane new_s = transform(s, tf); const Vec3f& n = new_s.n; const FCL_REAL& d = new_s.d; const std::size_t D = 9; for(std::size_t i = 0; i < D; ++i) bv.dist(i) = -std::numeric_limits::max(); for(std::size_t i = D; i < 2 * D; ++i) bv.dist(i) = std::numeric_limits::max(); if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { if(n[0] > 0) bv.dist(0) = bv.dist(D) = d; else bv.dist(0) = bv.dist(D) = -d; } else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { if(n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d; else bv.dist(1) = bv.dist(D + 1) = -d; } else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { if(n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d; else bv.dist(2) = bv.dist(D + 2) = -d; } else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1]) { bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2; } else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2]) { bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2; } else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2]) { bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2; } else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2; } else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2; } else if(n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) { bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2; } } template<> void computeBV, Plane>(const Plane& s, const Transform3f& tf, KDOP<24>& bv) { Plane new_s = transform(s, tf); const Vec3f& n = new_s.n; const FCL_REAL& d = new_s.d; const std::size_t D = 12; for(std::size_t i = 0; i < D; ++i) bv.dist(i) = -std::numeric_limits::max(); for(std::size_t i = D; i < 2 * D; ++i) bv.dist(i) = std::numeric_limits::max(); if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { if(n[0] > 0) bv.dist(0) = bv.dist(D) = d; else bv.dist(0) = bv.dist(D) = -d; } else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0) { if(n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d; else bv.dist(1) = bv.dist(D + 1) = -d; } else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0) { if(n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d; else bv.dist(2) = bv.dist(D + 2) = -d; } else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1]) { bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2; } else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2]) { bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2; } else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2]) { bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2; } else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2; } else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2; } else if(n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) { bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2; } else if(n[0] + n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0) { bv.dist(9) = bv.dist(D + 9) = n[0] * d * 3; } else if(n[0] + n[1] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0) { bv.dist(10) = bv.dist(D + 10) = n[0] * d * 3; } else if(n[0] + n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0) { bv.dist(11) = bv.dist(D + 11) = n[1] * d * 3; } } void constructBox(const AABB& bv, Box& box, Transform3f& tf) { box = Box(bv.max_ - bv.min_); tf = Transform3f(bv.center()); } void constructBox(const OBB& bv, Box& box, Transform3f& tf) { box = Box(bv.extent * 2); tf = Transform3f(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0], bv.axis[0][1], bv.axis[1][1], bv.axis[2][1], bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.To); } void constructBox(const OBBRSS& bv, Box& box, Transform3f& tf) { box = Box(bv.obb.extent * 2); tf = Transform3f(Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0], bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1], bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]), bv.obb.To); } void constructBox(const kIOS& bv, Box& box, Transform3f& tf) { box = Box(bv.obb.extent * 2); tf = Transform3f(Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0], bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1], bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]), bv.obb.To); } void constructBox(const RSS& bv, Box& box, Transform3f& tf) { box = Box(bv.width(), bv.height(), bv.depth()); tf = Transform3f(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0], bv.axis[0][1], bv.axis[1][1], bv.axis[2][1], bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.Tr); } void constructBox(const KDOP<16>& bv, Box& box, Transform3f& tf) { box = Box(bv.width(), bv.height(), bv.depth()); tf = Transform3f(bv.center()); } void constructBox(const KDOP<18>& bv, Box& box, Transform3f& tf) { box = Box(bv.width(), bv.height(), bv.depth()); tf = Transform3f(bv.center()); } void constructBox(const KDOP<24>& bv, Box& box, Transform3f& tf) { box = Box(bv.width(), bv.height(), bv.depth()); tf = Transform3f(bv.center()); } void constructBox(const AABB& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf) { box = Box(bv.max_ - bv.min_); tf = tf_bv * Transform3f(bv.center()); } void constructBox(const OBB& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf) { box = Box(bv.extent * 2); tf = tf_bv *Transform3f(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0], bv.axis[0][1], bv.axis[1][1], bv.axis[2][1], bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.To); } void constructBox(const OBBRSS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf) { box = Box(bv.obb.extent * 2); tf = tf_bv * Transform3f(Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0], bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1], bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]), bv.obb.To); } void constructBox(const kIOS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf) { box = Box(bv.obb.extent * 2); tf = tf_bv * Transform3f(Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0], bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1], bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]), bv.obb.To); } void constructBox(const RSS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf) { box = Box(bv.width(), bv.height(), bv.depth()); tf = tf_bv * Transform3f(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0], bv.axis[0][1], bv.axis[1][1], bv.axis[2][1], bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.Tr); } void constructBox(const KDOP<16>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf) { box = Box(bv.width(), bv.height(), bv.depth()); tf = tf_bv * Transform3f(bv.center()); } void constructBox(const KDOP<18>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf) { box = Box(bv.width(), bv.height(), bv.depth()); tf = tf_bv * Transform3f(bv.center()); } void constructBox(const KDOP<24>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf) { box = Box(bv.width(), bv.height(), bv.depth()); tf = tf_bv * Transform3f(bv.center()); } } fcl-0.5.0/src/traversal/000077500000000000000000000000001274356571000150645ustar00rootroot00000000000000fcl-0.5.0/src/traversal/traversal_node_base.cpp000066400000000000000000000057411274356571000216010ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/traversal/traversal_node_base.h" #include namespace fcl { TraversalNodeBase::~TraversalNodeBase() { } bool TraversalNodeBase::isFirstNodeLeaf(int b) const { return true; } bool TraversalNodeBase::isSecondNodeLeaf(int b) const { return true; } bool TraversalNodeBase::firstOverSecond(int b1, int b2) const { return true; } int TraversalNodeBase::getFirstLeftChild(int b) const { return b; } int TraversalNodeBase::getFirstRightChild(int b) const { return b; } int TraversalNodeBase::getSecondLeftChild(int b) const { return b; } int TraversalNodeBase::getSecondRightChild(int b) const { return b; } CollisionTraversalNodeBase::~CollisionTraversalNodeBase() { } bool CollisionTraversalNodeBase::BVTesting(int b1, int b2) const { return true; } void CollisionTraversalNodeBase::leafTesting(int b1, int b2) const { } bool CollisionTraversalNodeBase::canStop() const { return false; } DistanceTraversalNodeBase::~DistanceTraversalNodeBase() { } FCL_REAL DistanceTraversalNodeBase::BVTesting(int b1, int b2) const { return std::numeric_limits::max(); } void DistanceTraversalNodeBase::leafTesting(int b1, int b2) const { } bool DistanceTraversalNodeBase::canStop(FCL_REAL c) const { return false; } } fcl-0.5.0/src/traversal/traversal_node_bvhs.cpp000066400000000000000000000642411274356571000216310ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/traversal/traversal_node_bvhs.h" namespace fcl { namespace details { template static inline void meshCollisionOrientedNodeLeafTesting(int b1, int b2, const BVHModel* model1, const BVHModel* model2, Vec3f* vertices1, Vec3f* vertices2, Triangle* tri_indices1, Triangle* tri_indices2, const Matrix3f& R, const Vec3f& T, const Transform3f& tf1, const Transform3f& tf2, bool enable_statistics, FCL_REAL cost_density, int& num_leaf_tests, const CollisionRequest& request, CollisionResult& result) { if(enable_statistics) num_leaf_tests++; const BVNode& node1 = model1->getBV(b1); const BVNode& node2 = model2->getBV(b2); int primitive_id1 = node1.primitiveId(); int primitive_id2 = node2.primitiveId(); const Triangle& tri_id1 = tri_indices1[primitive_id1]; const Triangle& tri_id2 = tri_indices2[primitive_id2]; const Vec3f& p1 = vertices1[tri_id1[0]]; const Vec3f& p2 = vertices1[tri_id1[1]]; const Vec3f& p3 = vertices1[tri_id1[2]]; const Vec3f& q1 = vertices2[tri_id2[0]]; const Vec3f& q2 = vertices2[tri_id2[1]]; const Vec3f& q3 = vertices2[tri_id2[2]]; if(model1->isOccupied() && model2->isOccupied()) { bool is_intersect = false; if(!request.enable_contact) // only interested in collision or not { if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3, R, T)) { is_intersect = true; if(result.numContacts() < request.num_max_contacts) result.addContact(Contact(model1, model2, primitive_id1, primitive_id2)); } } else // need compute the contact information { FCL_REAL penetration; Vec3f normal; unsigned int n_contacts; Vec3f contacts[2]; if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3, R, T, contacts, &n_contacts, &penetration, &normal)) { is_intersect = true; if(request.num_max_contacts < result.numContacts() + n_contacts) n_contacts = (request.num_max_contacts > result.numContacts()) ? (request.num_max_contacts - result.numContacts()) : 0; for(unsigned int i = 0; i < n_contacts; ++i) { result.addContact(Contact(model1, model2, primitive_id1, primitive_id2, tf1.transform(contacts[i]), tf1.getQuatRotation().transform(normal), penetration)); } } } if(is_intersect && request.enable_cost) { AABB overlap_part; AABB(tf1.transform(p1), tf1.transform(p2), tf1.transform(p3)).overlap(AABB(tf2.transform(q1), tf2.transform(q2), tf2.transform(q3)), overlap_part); result.addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources); } } else if((!model1->isFree() && !model2->isFree()) && request.enable_cost) { if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3, R, T)) { AABB overlap_part; AABB(tf1.transform(p1), tf1.transform(p2), tf1.transform(p3)).overlap(AABB(tf2.transform(q1), tf2.transform(q2), tf2.transform(q3)), overlap_part); result.addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources); } } } template static inline void meshDistanceOrientedNodeLeafTesting(int b1, int b2, const BVHModel* model1, const BVHModel* model2, Vec3f* vertices1, Vec3f* vertices2, Triangle* tri_indices1, Triangle* tri_indices2, const Matrix3f& R, const Vec3f& T, bool enable_statistics, int& num_leaf_tests, const DistanceRequest& request, DistanceResult& result) { if(enable_statistics) num_leaf_tests++; const BVNode& node1 = model1->getBV(b1); const BVNode& node2 = model2->getBV(b2); int primitive_id1 = node1.primitiveId(); int primitive_id2 = node2.primitiveId(); const Triangle& tri_id1 = tri_indices1[primitive_id1]; const Triangle& tri_id2 = tri_indices2[primitive_id2]; const Vec3f& t11 = vertices1[tri_id1[0]]; const Vec3f& t12 = vertices1[tri_id1[1]]; const Vec3f& t13 = vertices1[tri_id1[2]]; const Vec3f& t21 = vertices2[tri_id2[0]]; const Vec3f& t22 = vertices2[tri_id2[1]]; const Vec3f& t23 = vertices2[tri_id2[2]]; // nearest point pair Vec3f P1, P2; FCL_REAL d = TriangleDistance::triDistance(t11, t12, t13, t21, t22, t23, R, T, P1, P2); if(request.enable_nearest_points) result.update(d, model1, model2, primitive_id1, primitive_id2, P1, P2); else result.update(d, model1, model2, primitive_id1, primitive_id2); } } MeshCollisionTraversalNodeOBB::MeshCollisionTraversalNodeOBB() : MeshCollisionTraversalNode() { R.setIdentity(); } bool MeshCollisionTraversalNodeOBB::BVTesting(int b1, int b2) const { if(enable_statistics) num_bv_tests++; return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv); } void MeshCollisionTraversalNodeOBB::leafTesting(int b1, int b2) const { details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2, R, T, tf1, tf2, enable_statistics, cost_density, num_leaf_tests, request, *result); } bool MeshCollisionTraversalNodeOBB::BVTesting(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc) const { if(enable_statistics) num_bv_tests++; return obbDisjoint(Rc, Tc, model1->getBV(b1).bv.extent, model2->getBV(b2).bv.extent); } void MeshCollisionTraversalNodeOBB::leafTesting(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc) const { details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2, R, T, tf1, tf2, enable_statistics, cost_density, num_leaf_tests, request, *result); } MeshCollisionTraversalNodeRSS::MeshCollisionTraversalNodeRSS() : MeshCollisionTraversalNode() { R.setIdentity(); } bool MeshCollisionTraversalNodeRSS::BVTesting(int b1, int b2) const { if(enable_statistics) num_bv_tests++; return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv); } void MeshCollisionTraversalNodeRSS::leafTesting(int b1, int b2) const { details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2, R, T, tf1, tf2, enable_statistics, cost_density, num_leaf_tests, request, *result); } MeshCollisionTraversalNodekIOS::MeshCollisionTraversalNodekIOS() : MeshCollisionTraversalNode() { R.setIdentity(); } bool MeshCollisionTraversalNodekIOS::BVTesting(int b1, int b2) const { if(enable_statistics) num_bv_tests++; return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv); } void MeshCollisionTraversalNodekIOS::leafTesting(int b1, int b2) const { details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2, R, T, tf1, tf2, enable_statistics, cost_density, num_leaf_tests, request, *result); } MeshCollisionTraversalNodeOBBRSS::MeshCollisionTraversalNodeOBBRSS() : MeshCollisionTraversalNode() { R.setIdentity(); } bool MeshCollisionTraversalNodeOBBRSS::BVTesting(int b1, int b2) const { if(enable_statistics) num_bv_tests++; return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv); } void MeshCollisionTraversalNodeOBBRSS::leafTesting(int b1, int b2) const { details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2, R, T, tf1, tf2, enable_statistics, cost_density, num_leaf_tests, request,*result); } namespace details { template static inline void distancePreprocessOrientedNode(const BVHModel* model1, const BVHModel* model2, const Vec3f* vertices1, Vec3f* vertices2, Triangle* tri_indices1, Triangle* tri_indices2, int init_tri_id1, int init_tri_id2, const Matrix3f& R, const Vec3f& T, const DistanceRequest& request, DistanceResult& result) { const Triangle& init_tri1 = tri_indices1[init_tri_id1]; const Triangle& init_tri2 = tri_indices2[init_tri_id2]; Vec3f init_tri1_points[3]; Vec3f init_tri2_points[3]; init_tri1_points[0] = vertices1[init_tri1[0]]; init_tri1_points[1] = vertices1[init_tri1[1]]; init_tri1_points[2] = vertices1[init_tri1[2]]; init_tri2_points[0] = vertices2[init_tri2[0]]; init_tri2_points[1] = vertices2[init_tri2[1]]; init_tri2_points[2] = vertices2[init_tri2[2]]; Vec3f p1, p2; FCL_REAL distance = TriangleDistance::triDistance(init_tri1_points[0], init_tri1_points[1], init_tri1_points[2], init_tri2_points[0], init_tri2_points[1], init_tri2_points[2], R, T, p1, p2); if(request.enable_nearest_points) result.update(distance, model1, model2, init_tri_id1, init_tri_id2, p1, p2); else result.update(distance, model1, model2, init_tri_id1, init_tri_id2); } template static inline void distancePostprocessOrientedNode(const BVHModel* model1, const BVHModel* model2, const Transform3f& tf1, const DistanceRequest& request, DistanceResult& result) { /// the points obtained by triDistance are not in world space: both are in object1's local coordinate system, so we need to convert them into the world space. if(request.enable_nearest_points && (result.o1 == model1) && (result.o2 == model2)) { result.nearest_points[0] = tf1.transform(result.nearest_points[0]); result.nearest_points[1] = tf1.transform(result.nearest_points[1]); } } } MeshDistanceTraversalNodeRSS::MeshDistanceTraversalNodeRSS() : MeshDistanceTraversalNode() { R.setIdentity(); } void MeshDistanceTraversalNodeRSS::preprocess() { details::distancePreprocessOrientedNode(model1, model2, vertices1, vertices2, tri_indices1, tri_indices2, 0, 0, R, T, request, *result); } void MeshDistanceTraversalNodeRSS::postprocess() { details::distancePostprocessOrientedNode(model1, model2, tf1, request, *result); } FCL_REAL MeshDistanceTraversalNodeRSS::BVTesting(int b1, int b2) const { if(enable_statistics) num_bv_tests++; return distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv); } void MeshDistanceTraversalNodeRSS::leafTesting(int b1, int b2) const { details::meshDistanceOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2, R, T, enable_statistics, num_leaf_tests, request, *result); } MeshDistanceTraversalNodekIOS::MeshDistanceTraversalNodekIOS() : MeshDistanceTraversalNode() { R.setIdentity(); } void MeshDistanceTraversalNodekIOS::preprocess() { details::distancePreprocessOrientedNode(model1, model2, vertices1, vertices2, tri_indices1, tri_indices2, 0, 0, R, T, request, *result); } void MeshDistanceTraversalNodekIOS::postprocess() { details::distancePostprocessOrientedNode(model1, model2, tf1, request, *result); } FCL_REAL MeshDistanceTraversalNodekIOS::BVTesting(int b1, int b2) const { if(enable_statistics) num_bv_tests++; return distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv); } void MeshDistanceTraversalNodekIOS::leafTesting(int b1, int b2) const { details::meshDistanceOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2, R, T, enable_statistics, num_leaf_tests, request, *result); } MeshDistanceTraversalNodeOBBRSS::MeshDistanceTraversalNodeOBBRSS() : MeshDistanceTraversalNode() { R.setIdentity(); } void MeshDistanceTraversalNodeOBBRSS::preprocess() { details::distancePreprocessOrientedNode(model1, model2, vertices1, vertices2, tri_indices1, tri_indices2, 0, 0, R, T, request, *result); } void MeshDistanceTraversalNodeOBBRSS::postprocess() { details::distancePostprocessOrientedNode(model1, model2, tf1, request, *result); } FCL_REAL MeshDistanceTraversalNodeOBBRSS::BVTesting(int b1, int b2) const { if(enable_statistics) num_bv_tests++; return distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv); } void MeshDistanceTraversalNodeOBBRSS::leafTesting(int b1, int b2) const { details::meshDistanceOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2, R, T, enable_statistics, num_leaf_tests, request, *result); } namespace details { template bool meshConservativeAdvancementOrientedNodeCanStop(FCL_REAL c, FCL_REAL min_distance, FCL_REAL abs_err, FCL_REAL rel_err, FCL_REAL w, const BVHModel* model1, const BVHModel* model2, const MotionBase* motion1, const MotionBase* motion2, std::vector& stack, FCL_REAL& delta_t) { if((c >= w * (min_distance - abs_err)) && (c * (1 + rel_err) >= w * min_distance)) { const ConservativeAdvancementStackData& data = stack.back(); FCL_REAL d = data.d; Vec3f n; int c1, c2; if(d > c) { const ConservativeAdvancementStackData& data2 = stack[stack.size() - 2]; d = data2.d; n = data2.P2 - data2.P1; n.normalize(); c1 = data2.c1; c2 = data2.c2; stack[stack.size() - 2] = stack[stack.size() - 1]; } else { n = data.P2 - data.P1; n.normalize(); c1 = data.c1; c2 = data.c2; } assert(c == d); // n is in local frame of c1, so we need to turn n into the global frame Vec3f n_transformed = getBVAxis(model1->getBV(c1).bv, 0) * n[0] + getBVAxis(model1->getBV(c1).bv, 1) * n[2] + getBVAxis(model1->getBV(c1).bv, 2) * n[2]; Quaternion3f R0; motion1->getCurrentRotation(R0); n_transformed = R0.transform(n_transformed); n_transformed.normalize(); TBVMotionBoundVisitor mb_visitor1(model1->getBV(c1).bv, n_transformed), mb_visitor2(model2->getBV(c2).bv, -n_transformed); FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1); FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2); FCL_REAL bound = bound1 + bound2; FCL_REAL cur_delta_t; if(bound <= c) cur_delta_t = 1; else cur_delta_t = c / bound; if(cur_delta_t < delta_t) delta_t = cur_delta_t; stack.pop_back(); return true; } else { const ConservativeAdvancementStackData& data = stack.back(); FCL_REAL d = data.d; if(d > c) stack[stack.size() - 2] = stack[stack.size() - 1]; stack.pop_back(); return false; } } template void meshConservativeAdvancementOrientedNodeLeafTesting(int b1, int b2, const BVHModel* model1, const BVHModel* model2, const Triangle* tri_indices1, const Triangle* tri_indices2, const Vec3f* vertices1, const Vec3f* vertices2, const Matrix3f& R, const Vec3f& T, const MotionBase* motion1, const MotionBase* motion2, bool enable_statistics, FCL_REAL& min_distance, Vec3f& p1, Vec3f& p2, int& last_tri_id1, int& last_tri_id2, FCL_REAL& delta_t, int& num_leaf_tests) { if(enable_statistics) num_leaf_tests++; const BVNode& node1 = model1->getBV(b1); const BVNode& node2 = model2->getBV(b2); int primitive_id1 = node1.primitiveId(); int primitive_id2 = node2.primitiveId(); const Triangle& tri_id1 = tri_indices1[primitive_id1]; const Triangle& tri_id2 = tri_indices2[primitive_id2]; const Vec3f& t11 = vertices1[tri_id1[0]]; const Vec3f& t12 = vertices1[tri_id1[1]]; const Vec3f& t13 = vertices1[tri_id1[2]]; const Vec3f& t21 = vertices2[tri_id2[0]]; const Vec3f& t22 = vertices2[tri_id2[1]]; const Vec3f& t23 = vertices2[tri_id2[2]]; // nearest point pair Vec3f P1, P2; FCL_REAL d = TriangleDistance::triDistance(t11, t12, t13, t21, t22, t23, R, T, P1, P2); if(d < min_distance) { min_distance = d; p1 = P1; p2 = P2; last_tri_id1 = primitive_id1; last_tri_id2 = primitive_id2; } /// n is the local frame of object 1, pointing from object 1 to object2 Vec3f n = P2 - P1; /// turn n into the global frame, pointing from object 1 to object 2 Quaternion3f R0; motion1->getCurrentRotation(R0); Vec3f n_transformed = R0.transform(n); n_transformed.normalize(); // normalized here TriangleMotionBoundVisitor mb_visitor1(t11, t12, t13, n_transformed), mb_visitor2(t21, t22, t23, -n_transformed); FCL_REAL bound1 = motion1->computeMotionBound(mb_visitor1); FCL_REAL bound2 = motion2->computeMotionBound(mb_visitor2); FCL_REAL bound = bound1 + bound2; FCL_REAL cur_delta_t; if(bound <= d) cur_delta_t = 1; else cur_delta_t = d / bound; if(cur_delta_t < delta_t) delta_t = cur_delta_t; } } MeshConservativeAdvancementTraversalNodeRSS::MeshConservativeAdvancementTraversalNodeRSS(FCL_REAL w_) : MeshConservativeAdvancementTraversalNode(w_) { R.setIdentity(); } FCL_REAL MeshConservativeAdvancementTraversalNodeRSS::BVTesting(int b1, int b2) const { if(enable_statistics) num_bv_tests++; Vec3f P1, P2; FCL_REAL d = distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv, &P1, &P2); stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d)); return d; } void MeshConservativeAdvancementTraversalNodeRSS::leafTesting(int b1, int b2) const { details::meshConservativeAdvancementOrientedNodeLeafTesting(b1, b2, model1, model2, tri_indices1, tri_indices2, vertices1, vertices2, R, T, motion1, motion2, enable_statistics, min_distance, closest_p1, closest_p2, last_tri_id1, last_tri_id2, delta_t, num_leaf_tests); } bool MeshConservativeAdvancementTraversalNodeRSS::canStop(FCL_REAL c) const { return details::meshConservativeAdvancementOrientedNodeCanStop(c, min_distance, abs_err, rel_err, w, model1, model2, motion1, motion2, stack, delta_t); } MeshConservativeAdvancementTraversalNodeOBBRSS::MeshConservativeAdvancementTraversalNodeOBBRSS(FCL_REAL w_) : MeshConservativeAdvancementTraversalNode(w_) { R.setIdentity(); } FCL_REAL MeshConservativeAdvancementTraversalNodeOBBRSS::BVTesting(int b1, int b2) const { if(enable_statistics) num_bv_tests++; Vec3f P1, P2; FCL_REAL d = distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv, &P1, &P2); stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d)); return d; } void MeshConservativeAdvancementTraversalNodeOBBRSS::leafTesting(int b1, int b2) const { details::meshConservativeAdvancementOrientedNodeLeafTesting(b1, b2, model1, model2, tri_indices1, tri_indices2, vertices1, vertices2, R, T, motion1, motion2, enable_statistics, min_distance, closest_p1, closest_p2, last_tri_id1, last_tri_id2, delta_t, num_leaf_tests); } bool MeshConservativeAdvancementTraversalNodeOBBRSS::canStop(FCL_REAL c) const { return details::meshConservativeAdvancementOrientedNodeCanStop(c, min_distance, abs_err, rel_err, w, model1, model2, motion1, motion2, stack, delta_t); } } fcl-0.5.0/src/traversal/traversal_node_setup.cpp000066400000000000000000000203431274356571000220220ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/traversal/traversal_node_setup.h" namespace fcl { namespace details { template static inline bool setupMeshCollisionOrientedNode(OrientedNode& node, const BVHModel& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result) { if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES) return false; node.vertices1 = model1.vertices; node.vertices2 = model2.vertices; node.tri_indices1 = model1.tri_indices; node.tri_indices2 = model2.tri_indices; node.model1 = &model1; node.tf1 = tf1; node.model2 = &model2; node.tf2 = tf2; node.request = request; node.result = &result; node.cost_density = model1.cost_density * model2.cost_density; relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.R, node.T); return true; } } bool initialize(MeshCollisionTraversalNodeOBB& node, const BVHModel& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result) { return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, request, result); } bool initialize(MeshCollisionTraversalNodeRSS& node, const BVHModel& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result) { return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, request, result); } bool initialize(MeshCollisionTraversalNodekIOS& node, const BVHModel& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result) { return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, request, result); } bool initialize(MeshCollisionTraversalNodeOBBRSS& node, const BVHModel& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result) { return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, request, result); } namespace details { template static inline bool setupMeshDistanceOrientedNode(OrientedNode& node, const BVHModel& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, const DistanceRequest& request, DistanceResult& result) { if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES) return false; node.request = request; node.result = &result; node.model1 = &model1; node.tf1 = tf1; node.model2 = &model2; node.tf2 = tf2; node.vertices1 = model1.vertices; node.vertices2 = model2.vertices; node.tri_indices1 = model1.tri_indices; node.tri_indices2 = model2.tri_indices; relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.R, node.T); return true; } } bool initialize(MeshDistanceTraversalNodeRSS& node, const BVHModel& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, const DistanceRequest& request, DistanceResult& result) { return details::setupMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, request, result); } bool initialize(MeshDistanceTraversalNodekIOS& node, const BVHModel& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, const DistanceRequest& request, DistanceResult& result) { return details::setupMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, request, result); } bool initialize(MeshDistanceTraversalNodeOBBRSS& node, const BVHModel& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, const DistanceRequest& request, DistanceResult& result) { return details::setupMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, request, result); } namespace details { template static inline bool setupMeshConservativeAdvancementOrientedDistanceNode(OrientedDistanceNode& node, const BVHModel& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, FCL_REAL w) { if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES) return false; node.model1 = &model1; node.model2 = &model2; node.vertices1 = model1.vertices; node.vertices2 = model2.vertices; node.tri_indices1 = model1.tri_indices; node.tri_indices2 = model2.tri_indices; node.w = w; relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.R, node.T); return true; } } bool initialize(MeshConservativeAdvancementTraversalNodeRSS& node, const BVHModel& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, FCL_REAL w) { return details::setupMeshConservativeAdvancementOrientedDistanceNode(node, model1, tf1, model2, tf2, w); } bool initialize(MeshConservativeAdvancementTraversalNodeOBBRSS& node, const BVHModel& model1, const Transform3f& tf1, const BVHModel& model2, const Transform3f& tf2, FCL_REAL w) { return details::setupMeshConservativeAdvancementOrientedDistanceNode(node, model1, tf1, model2, tf2, w); } } fcl-0.5.0/src/traversal/traversal_recurse.cpp000066400000000000000000000262051274356571000213300ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #include "fcl/traversal/traversal_recurse.h" namespace fcl { void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list) { bool l1 = node->isFirstNodeLeaf(b1); bool l2 = node->isSecondNodeLeaf(b2); if(l1 && l2) { updateFrontList(front_list, b1, b2); if(node->BVTesting(b1, b2)) return; node->leafTesting(b1, b2); return; } if(node->BVTesting(b1, b2)) { updateFrontList(front_list, b1, b2); return; } if(node->firstOverSecond(b1, b2)) { int c1 = node->getFirstLeftChild(b1); int c2 = node->getFirstRightChild(b1); collisionRecurse(node, c1, b2, front_list); // early stop is disabled is front_list is used if(node->canStop() && !front_list) return; collisionRecurse(node, c2, b2, front_list); } else { int c1 = node->getSecondLeftChild(b2); int c2 = node->getSecondRightChild(b2); collisionRecurse(node, b1, c1, front_list); // early stop is disabled is front_list is used if(node->canStop() && !front_list) return; collisionRecurse(node, b1, c2, front_list); } } void collisionRecurse(MeshCollisionTraversalNodeOBB* node, int b1, int b2, const Matrix3f& R, const Vec3f& T, BVHFrontList* front_list) { bool l1 = node->isFirstNodeLeaf(b1); bool l2 = node->isSecondNodeLeaf(b2); if(l1 && l2) { updateFrontList(front_list, b1, b2); if(node->BVTesting(b1, b2, R, T)) return; node->leafTesting(b1, b2, R, T); return; } if(node->BVTesting(b1, b2, R, T)) { updateFrontList(front_list, b1, b2); return; } Vec3f temp; if(node->firstOverSecond(b1, b2)) { int c1 = node->getFirstLeftChild(b1); int c2 = node->getFirstRightChild(b1); const OBB& bv1 = node->model1->getBV(c1).bv; Matrix3f Rc(R.transposeTimes(bv1.axis[0]), R.transposeTimes(bv1.axis[1]), R.transposeTimes(bv1.axis[2])); temp = T - bv1.To; Vec3f Tc(temp.dot(bv1.axis[0]), temp.dot(bv1.axis[1]), temp.dot(bv1.axis[2])); collisionRecurse(node, c1, b2, Rc, Tc, front_list); // early stop is disabled is front_list is used if(node->canStop() && !front_list) return; const OBB& bv2 = node->model1->getBV(c2).bv; Rc = Matrix3f(R.transposeTimes(bv2.axis[0]), R.transposeTimes(bv2.axis[1]), R.transposeTimes(bv2.axis[2])); temp = T - bv2.To; Tc.setValue(temp.dot(bv2.axis[0]), temp.dot(bv2.axis[1]), temp.dot(bv2.axis[2])); collisionRecurse(node, c2, b2, Rc, Tc, front_list); } else { int c1 = node->getSecondLeftChild(b2); int c2 = node->getSecondRightChild(b2); const OBB& bv1 = node->model2->getBV(c1).bv; Matrix3f Rc; temp = R * bv1.axis[0]; Rc(0, 0) = temp[0]; Rc(1, 0) = temp[1]; Rc(2, 0) = temp[2]; temp = R * bv1.axis[1]; Rc(0, 1) = temp[0]; Rc(1, 1) = temp[1]; Rc(2, 1) = temp[2]; temp = R * bv1.axis[2]; Rc(0, 2) = temp[0]; Rc(1, 2) = temp[1]; Rc(2, 2) = temp[2]; Vec3f Tc = R * bv1.To + T; collisionRecurse(node, b1, c1, Rc, Tc, front_list); // early stop is disabled is front_list is used if(node->canStop() && !front_list) return; const OBB& bv2 = node->model2->getBV(c2).bv; temp = R * bv2.axis[0]; Rc(0, 0) = temp[0]; Rc(1, 0) = temp[1]; Rc(2, 0) = temp[2]; temp = R * bv2.axis[1]; Rc(0, 1) = temp[0]; Rc(1, 1) = temp[1]; Rc(2, 1) = temp[2]; temp = R * bv2.axis[2]; Rc(0, 2) = temp[0]; Rc(1, 2) = temp[1]; Rc(2, 2) = temp[2]; Tc = R * bv2.To + T; collisionRecurse(node, b1, c2, Rc, Tc, front_list); } } void collisionRecurse(MeshCollisionTraversalNodeRSS* node, int b1, int b2, const Matrix3f& R, const Vec3f& T, BVHFrontList* front_list) { } /** Recurse function for self collision * Make sure node is set correctly so that the first and second tree are the same */ void selfCollisionRecurse(CollisionTraversalNodeBase* node, int b, BVHFrontList* front_list) { bool l = node->isFirstNodeLeaf(b); if(l) return; int c1 = node->getFirstLeftChild(b); int c2 = node->getFirstRightChild(b); selfCollisionRecurse(node, c1, front_list); if(node->canStop() && !front_list) return; selfCollisionRecurse(node, c2, front_list); if(node->canStop() && !front_list) return; collisionRecurse(node, c1, c2, front_list); } void distanceRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list) { bool l1 = node->isFirstNodeLeaf(b1); bool l2 = node->isSecondNodeLeaf(b2); if(l1 && l2) { updateFrontList(front_list, b1, b2); node->leafTesting(b1, b2); return; } int a1, a2, c1, c2; if(node->firstOverSecond(b1, b2)) { a1 = node->getFirstLeftChild(b1); a2 = b2; c1 = node->getFirstRightChild(b1); c2 = b2; } else { a1 = b1; a2 = node->getSecondLeftChild(b2); c1 = b1; c2 = node->getSecondRightChild(b2); } FCL_REAL d1 = node->BVTesting(a1, a2); FCL_REAL d2 = node->BVTesting(c1, c2); if(d2 < d1) { if(!node->canStop(d2)) distanceRecurse(node, c1, c2, front_list); else updateFrontList(front_list, c1, c2); if(!node->canStop(d1)) distanceRecurse(node, a1, a2, front_list); else updateFrontList(front_list, a1, a2); } else { if(!node->canStop(d1)) distanceRecurse(node, a1, a2, front_list); else updateFrontList(front_list, a1, a2); if(!node->canStop(d2)) distanceRecurse(node, c1, c2, front_list); else updateFrontList(front_list, c1, c2); } } /** \brief Bounding volume test structure */ struct BVT { /** \brief distance between bvs */ FCL_REAL d; /** \brief bv indices for a pair of bvs in two models */ int b1, b2; }; /** \brief Comparer between two BVT */ struct BVT_Comparer { bool operator() (const BVT& lhs, const BVT& rhs) const { return lhs.d > rhs.d; } }; struct BVTQ { BVTQ() : qsize(2) {} bool empty() const { return pq.empty(); } size_t size() const { return pq.size(); } const BVT& top() const { return pq.top(); } void push(const BVT& x) { pq.push(x); } void pop() { pq.pop(); } bool full() const { return (pq.size() + 1 >= qsize); } std::priority_queue, BVT_Comparer> pq; /** \brief Queue size */ unsigned int qsize; }; void distanceQueueRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list, int qsize) { BVTQ bvtq; bvtq.qsize = qsize; BVT min_test; min_test.b1 = b1; min_test.b2 = b2; while(1) { bool l1 = node->isFirstNodeLeaf(min_test.b1); bool l2 = node->isSecondNodeLeaf(min_test.b2); if(l1 && l2) { updateFrontList(front_list, min_test.b1, min_test.b2); node->leafTesting(min_test.b1, min_test.b2); } else if(bvtq.full()) { // queue should not get two more tests, recur distanceQueueRecurse(node, min_test.b1, min_test.b2, front_list, qsize); } else { // queue capacity is not full yet BVT bvt1, bvt2; if(node->firstOverSecond(min_test.b1, min_test.b2)) { int c1 = node->getFirstLeftChild(min_test.b1); int c2 = node->getFirstRightChild(min_test.b1); bvt1.b1 = c1; bvt1.b2 = min_test.b2; bvt1.d = node->BVTesting(bvt1.b1, bvt1.b2); bvt2.b1 = c2; bvt2.b2 = min_test.b2; bvt2.d = node->BVTesting(bvt2.b1, bvt2.b2); } else { int c1 = node->getSecondLeftChild(min_test.b2); int c2 = node->getSecondRightChild(min_test.b2); bvt1.b1 = min_test.b1; bvt1.b2 = c1; bvt1.d = node->BVTesting(bvt1.b1, bvt1.b2); bvt2.b1 = min_test.b1; bvt2.b2 = c2; bvt2.d = node->BVTesting(bvt2.b1, bvt2.b2); } bvtq.push(bvt1); bvtq.push(bvt2); } if(bvtq.empty()) break; else { min_test = bvtq.top(); bvtq.pop(); if(node->canStop(min_test.d)) { updateFrontList(front_list, min_test.b1, min_test.b2); break; } } } } void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node, BVHFrontList* front_list) { BVHFrontList::iterator front_iter; BVHFrontList append; for(front_iter = front_list->begin(); front_iter != front_list->end(); ++front_iter) { int b1 = front_iter->left; int b2 = front_iter->right; bool l1 = node->isFirstNodeLeaf(b1); bool l2 = node->isSecondNodeLeaf(b2); if(l1 & l2) { front_iter->valid = false; // the front node is no longer valid, in collideRecurse will add again. collisionRecurse(node, b1, b2, &append); } else { if(!node->BVTesting(b1, b2)) { front_iter->valid = false; if(node->firstOverSecond(b1, b2)) { int c1 = node->getFirstLeftChild(b1); int c2 = node->getFirstRightChild(b1); collisionRecurse(node, c1, b2, front_list); collisionRecurse(node, c2, b2, front_list); } else { int c1 = node->getSecondLeftChild(b2); int c2 = node->getSecondRightChild(b2); collisionRecurse(node, b1, c1, front_list); collisionRecurse(node, b1, c2, front_list); } } } } // clean the old front list (remove invalid node) for(front_iter = front_list->begin(); front_iter != front_list->end();) { if(!front_iter->valid) front_iter = front_list->erase(front_iter); else ++front_iter; } for(front_iter = append.begin(); front_iter != append.end(); ++front_iter) { front_list->push_back(*front_iter); } } } fcl-0.5.0/test/000077500000000000000000000000001274356571000132515ustar00rootroot00000000000000fcl-0.5.0/test/CMakeLists.txt000066400000000000000000000052031274356571000160110ustar00rootroot00000000000000include_directories(${Boost_INCLUDE_DIR}) if(MSVC) add_definitions(-DBOOST_ALL_NO_LIB) endif() add_definitions(-DBOOST_TEST_DYN_LINK) macro(add_fcl_test test_name) add_executable(${ARGV}) target_link_libraries(${test_name} fcl ${Boost_FILESYSTEM_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${Boost_THREAD_LIBRARY} ${Boost_DATE_TIME_LIBRARY} ${Boost_UNIT_TEST_FRAMEWORK_LIBRARY}) add_test(${test_name} ${EXECUTABLE_OUTPUT_PATH}/${test_name}) endmacro(add_fcl_test) # configure location of resources file(TO_NATIVE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/fcl_resources" TEST_RESOURCES_SRC_DIR) file(TO_NATIVE_PATH "${CMAKE_CURRENT_BINARY_DIR}/fcl_resources" TEST_RESOURCES_BIN_DIR) if(WIN32) # Correct directory separator for Windows string(REPLACE "\\" "\\\\" TEST_RESOURCES_SRC_DIR ${TEST_RESOURCES_SRC_DIR}) string(REPLACE "\\" "\\\\" TEST_RESOURCES_BIN_DIR ${TEST_RESOURCES_BIN_DIR}) endif(WIN32) configure_file("${TEST_RESOURCES_SRC_DIR}/config.h.in" "${TEST_RESOURCES_BIN_DIR}/config.h") include_directories(.) include_directories("${CMAKE_CURRENT_BINARY_DIR}") add_fcl_test(test_fcl_collision test_fcl_collision.cpp test_fcl_utility.cpp) add_fcl_test(test_fcl_distance test_fcl_distance.cpp test_fcl_utility.cpp) add_fcl_test(test_fcl_geometric_shapes test_fcl_geometric_shapes.cpp test_fcl_utility.cpp) add_fcl_test(test_fcl_broadphase test_fcl_broadphase.cpp test_fcl_utility.cpp) add_fcl_test(test_fcl_shape_mesh_consistency test_fcl_shape_mesh_consistency.cpp test_fcl_utility.cpp) add_fcl_test(test_fcl_frontlist test_fcl_frontlist.cpp test_fcl_utility.cpp) add_fcl_test(test_fcl_math test_fcl_math.cpp test_fcl_utility.cpp) add_fcl_test(test_fcl_sphere_capsule test_fcl_sphere_capsule.cpp) add_fcl_test(test_fcl_capsule_capsule test_fcl_capsule_capsule.cpp) add_fcl_test(test_fcl_simple test_fcl_simple.cpp) add_fcl_test(test_fcl_capsule_box_1 test_fcl_capsule_box_1.cpp) add_fcl_test(test_fcl_capsule_box_2 test_fcl_capsule_box_2.cpp) #add_fcl_test(test_fcl_global_penetration test_fcl_global_penetration.cpp libsvm/svm.cpp test_fcl_utility.cpp) add_fcl_test(test_fcl_bvh_models test_fcl_bvh_models.cpp test_fcl_utility.cpp) if (FCL_HAVE_OCTOMAP) add_fcl_test(test_fcl_octomap test_fcl_octomap.cpp test_fcl_utility.cpp) endif() #if (FCL_HAVE_TINYXML) # add_executable(test_fcl_xmldata test_fcl_xmldata.cpp test_fcl_utility.cpp libsvm/svm.cpp) # target_link_libraries(test_fcl_xmldata # fcl # ${TINYXML_LIBRARY_DIRS} # ${Boost_SYSTEM_LIBRARY} # ${Boost_THREAD_LIBRARY} # ${Boost_DATE_TIME_LIBRARY} # ${Boost_UNIT_TEST_FRAMEWORK_LIBRARY}) # add_test(test_fcl_xmldata ${EXECUTABLE_OUTPUT_PATH}/test_fcl_xmldata) #endif() fcl-0.5.0/test/fcl_resources/000077500000000000000000000000001274356571000161075ustar00rootroot00000000000000fcl-0.5.0/test/fcl_resources/config.h.in000066400000000000000000000034441274356571000201370ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2010-2016, Rice University. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of the Rice University 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 OWNER 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. */ /* Author: Mark Moll */ #ifndef FCL_TEST_RESOURCES_CONFIG_ #define FCL_TEST_RESOURCES_CONFIG_ #define TEST_RESOURCES_DIR "@TEST_RESOURCES_SRC_DIR@" #endif fcl-0.5.0/test/fcl_resources/env.obj000066400000000000000000005536021274356571000174060ustar00rootroot000000000000006540 2180 v -2987.5 -3004.5 2987.5 v 987.5 -3004.5 2987.5 v -2987.5 -2995.5 2987.5 v -2987.5 -2995.5 2987.5 v 987.5 -3004.5 2987.5 v 987.5 -2995.5 2987.5 v 987.5 -3004.5 12.5 v -2987.5 -3004.5 12.5 v 987.5 -2995.5 12.5 v 987.5 -2995.5 12.5 v -2987.5 -3004.5 12.5 v -2987.5 -2995.5 12.5 v 987.5 -3004.5 2987.5 v 987.5 -3004.5 12.5 v 987.5 -2995.5 2987.5 v 987.5 -2995.5 2987.5 v 987.5 -3004.5 12.5 v 987.5 -2995.5 12.5 v -2987.5 -3004.5 12.5 v -2987.5 -3004.5 2987.5 v -2987.5 -2995.5 12.5 v -2987.5 -2995.5 12.5 v -2987.5 -3004.5 2987.5 v -2987.5 -2995.5 2987.5 v -2987.5 -2995.5 12.5 v -2987.5 -2995.5 2987.5 v 987.5 -2995.5 12.5 v 987.5 -2995.5 12.5 v -2987.5 -2995.5 2987.5 v 987.5 -2995.5 2987.5 v -2987.5 -3004.5 12.5 v 987.5 -3004.5 12.5 v -2987.5 -3004.5 2987.5 v -2987.5 -3004.5 2987.5 v 987.5 -3004.5 12.5 v 987.5 -3004.5 2987.5 v -2987.5 -2987.5 -5.5 v 2987.5 -2987.5 -5.5 v -2987.5 2987.5 -5.5 v -2987.5 2987.5 -5.5 v 2987.5 -2987.5 -5.5 v 2987.5 2987.5 -5.5 v 2987.5 -2987.5 -14.5 v -2987.5 -2987.5 -14.5 v 2987.5 2987.5 -14.5 v 2987.5 2987.5 -14.5 v -2987.5 -2987.5 -14.5 v -2987.5 2987.5 -14.5 v 2987.5 -2987.5 -5.5 v 2987.5 -2987.5 -14.5 v 2987.5 2987.5 -5.5 v 2987.5 2987.5 -5.5 v 2987.5 -2987.5 -14.5 v 2987.5 2987.5 -14.5 v -2987.5 -2987.5 -14.5 v -2987.5 -2987.5 -5.5 v -2987.5 2987.5 -14.5 v -2987.5 2987.5 -14.5 v -2987.5 -2987.5 -5.5 v -2987.5 2987.5 -5.5 v -2987.5 2987.5 -14.5 v -2987.5 2987.5 -5.5 v 2987.5 2987.5 -14.5 v 2987.5 2987.5 -14.5 v -2987.5 2987.5 -5.5 v 2987.5 2987.5 -5.5 v -2987.5 -2987.5 -14.5 v 2987.5 -2987.5 -14.5 v -2987.5 -2987.5 -5.5 v -2987.5 -2987.5 -5.5 v 2987.5 -2987.5 -14.5 v 2987.5 -2987.5 -5.5 v 995.5 -2987.5 987.5 v 1004.5 -2987.5 987.5 v 995.5 2987.5 987.5 v 995.5 2987.5 987.5 v 1004.5 -2987.5 987.5 v 1004.5 2987.5 987.5 v 1004.5 -2987.5 12.5 v 995.5 -2987.5 12.5 v 1004.5 2987.5 12.5 v 1004.5 2987.5 12.5 v 995.5 -2987.5 12.5 v 995.5 2987.5 12.5 v 1004.5 -2987.5 987.5 v 1004.5 -2987.5 12.5 v 1004.5 2987.5 987.5 v 1004.5 2987.5 987.5 v 1004.5 -2987.5 12.5 v 1004.5 2987.5 12.5 v 995.5 -2987.5 12.5 v 995.5 -2987.5 987.5 v 995.5 2987.5 12.5 v 995.5 2987.5 12.5 v 995.5 -2987.5 987.5 v 995.5 2987.5 987.5 v 995.5 2987.5 12.5 v 995.5 2987.5 987.5 v 1004.5 2987.5 12.5 v 1004.5 2987.5 12.5 v 995.5 2987.5 987.5 v 1004.5 2987.5 987.5 v 995.5 -2987.5 12.5 v 1004.5 -2987.5 12.5 v 995.5 -2987.5 987.5 v 995.5 -2987.5 987.5 v 1004.5 -2987.5 12.5 v 1004.5 -2987.5 987.5 v 995.5 -2987.5 2987.5 v 1004.5 -2987.5 2987.5 v 995.5 2987.5 2987.5 v 995.5 2987.5 2987.5 v 1004.5 -2987.5 2987.5 v 1004.5 2987.5 2987.5 v 1004.5 -2987.5 2012.5 v 995.5 -2987.5 2012.5 v 1004.5 2987.5 2012.5 v 1004.5 2987.5 2012.5 v 995.5 -2987.5 2012.5 v 995.5 2987.5 2012.5 v 1004.5 -2987.5 2987.5 v 1004.5 -2987.5 2012.5 v 1004.5 2987.5 2987.5 v 1004.5 2987.5 2987.5 v 1004.5 -2987.5 2012.5 v 1004.5 2987.5 2012.5 v 995.5 -2987.5 2012.5 v 995.5 -2987.5 2987.5 v 995.5 2987.5 2012.5 v 995.5 2987.5 2012.5 v 995.5 -2987.5 2987.5 v 995.5 2987.5 2987.5 v 995.5 2987.5 2012.5 v 995.5 2987.5 2987.5 v 1004.5 2987.5 2012.5 v 1004.5 2987.5 2012.5 v 995.5 2987.5 2987.5 v 1004.5 2987.5 2987.5 v 995.5 -2987.5 2012.5 v 1004.5 -2987.5 2012.5 v 995.5 -2987.5 2987.5 v 995.5 -2987.5 2987.5 v 1004.5 -2987.5 2012.5 v 1004.5 -2987.5 2987.5 v 995.5 -2987.5 1987.5 v 1004.5 -2987.5 1987.5 v 995.5 -1012.5 1987.5 v 995.5 -1012.5 1987.5 v 1004.5 -2987.5 1987.5 v 1004.5 -1012.5 1987.5 v 1004.5 -2987.5 1012.5 v 995.5 -2987.5 1012.5 v 1004.5 -1012.5 1012.5 v 1004.5 -1012.5 1012.5 v 995.5 -2987.5 1012.5 v 995.5 -1012.5 1012.5 v 1004.5 -2987.5 1987.5 v 1004.5 -2987.5 1012.5 v 1004.5 -1012.5 1987.5 v 1004.5 -1012.5 1987.5 v 1004.5 -2987.5 1012.5 v 1004.5 -1012.5 1012.5 v 995.5 -2987.5 1012.5 v 995.5 -2987.5 1987.5 v 995.5 -1012.5 1012.5 v 995.5 -1012.5 1012.5 v 995.5 -2987.5 1987.5 v 995.5 -1012.5 1987.5 v 995.5 -1012.5 1012.5 v 995.5 -1012.5 1987.5 v 1004.5 -1012.5 1012.5 v 1004.5 -1012.5 1012.5 v 995.5 -1012.5 1987.5 v 1004.5 -1012.5 1987.5 v 995.5 -2987.5 1012.5 v 1004.5 -2987.5 1012.5 v 995.5 -2987.5 1987.5 v 995.5 -2987.5 1987.5 v 1004.5 -2987.5 1012.5 v 1004.5 -2987.5 1987.5 v 995.5 1012.5 1987.5 v 1004.5 1012.5 1987.5 v 995.5 2987.5 1987.5 v 995.5 2987.5 1987.5 v 1004.5 1012.5 1987.5 v 1004.5 2987.5 1987.5 v 1004.5 1012.5 1012.5 v 995.5 1012.5 1012.5 v 1004.5 2987.5 1012.5 v 1004.5 2987.5 1012.5 v 995.5 1012.5 1012.5 v 995.5 2987.5 1012.5 v 1004.5 1012.5 1987.5 v 1004.5 1012.5 1012.5 v 1004.5 2987.5 1987.5 v 1004.5 2987.5 1987.5 v 1004.5 1012.5 1012.5 v 1004.5 2987.5 1012.5 v 995.5 1012.5 1012.5 v 995.5 1012.5 1987.5 v 995.5 2987.5 1012.5 v 995.5 2987.5 1012.5 v 995.5 1012.5 1987.5 v 995.5 2987.5 1987.5 v 995.5 2987.5 1012.5 v 995.5 2987.5 1987.5 v 1004.5 2987.5 1012.5 v 1004.5 2987.5 1012.5 v 995.5 2987.5 1987.5 v 1004.5 2987.5 1987.5 v 995.5 1012.5 1012.5 v 1004.5 1012.5 1012.5 v 995.5 1012.5 1987.5 v 995.5 1012.5 1987.5 v 1004.5 1012.5 1012.5 v 1004.5 1012.5 1987.5 v -2987.5 2995.5 2987.5 v 987.5 2995.5 2987.5 v -2987.5 3004.5 2987.5 v -2987.5 3004.5 2987.5 v 987.5 2995.5 2987.5 v 987.5 3004.5 2987.5 v 987.5 2995.5 12.5 v -2987.5 2995.5 12.5 v 987.5 3004.5 12.5 v 987.5 3004.5 12.5 v -2987.5 2995.5 12.5 v -2987.5 3004.5 12.5 v 987.5 2995.5 2987.5 v 987.5 2995.5 12.5 v 987.5 3004.5 2987.5 v 987.5 3004.5 2987.5 v 987.5 2995.5 12.5 v 987.5 3004.5 12.5 v -2987.5 2995.5 12.5 v -2987.5 2995.5 2987.5 v -2987.5 3004.5 12.5 v -2987.5 3004.5 12.5 v -2987.5 2995.5 2987.5 v -2987.5 3004.5 2987.5 v -2987.5 3004.5 12.5 v -2987.5 3004.5 2987.5 v 987.5 3004.5 12.5 v 987.5 3004.5 12.5 v -2987.5 3004.5 2987.5 v 987.5 3004.5 2987.5 v -2987.5 2995.5 12.5 v 987.5 2995.5 12.5 v -2987.5 2995.5 2987.5 v -2987.5 2995.5 2987.5 v 987.5 2995.5 12.5 v 987.5 2995.5 2987.5 v -3004.5 -2987.5 2987.5 v -2995.5 -2987.5 2987.5 v -3004.5 2987.5 2987.5 v -3004.5 2987.5 2987.5 v -2995.5 -2987.5 2987.5 v -2995.5 2987.5 2987.5 v -2995.5 -2987.5 12.5 v -3004.5 -2987.5 12.5 v -2995.5 2987.5 12.5 v -2995.5 2987.5 12.5 v -3004.5 -2987.5 12.5 v -3004.5 2987.5 12.5 v -2995.5 -2987.5 2987.5 v -2995.5 -2987.5 12.5 v -2995.5 2987.5 2987.5 v -2995.5 2987.5 2987.5 v -2995.5 -2987.5 12.5 v -2995.5 2987.5 12.5 v -3004.5 -2987.5 12.5 v -3004.5 -2987.5 2987.5 v -3004.5 2987.5 12.5 v -3004.5 2987.5 12.5 v -3004.5 -2987.5 2987.5 v -3004.5 2987.5 2987.5 v -3004.5 2987.5 12.5 v -3004.5 2987.5 2987.5 v -2995.5 2987.5 12.5 v -2995.5 2987.5 12.5 v -3004.5 2987.5 2987.5 v -2995.5 2987.5 2987.5 v -3004.5 -2987.5 12.5 v -2995.5 -2987.5 12.5 v -3004.5 -2987.5 2987.5 v -3004.5 -2987.5 2987.5 v -2995.5 -2987.5 12.5 v -2995.5 -2987.5 2987.5 v -1660 -2650 495 v -1659.24 -2653.83 495 v -1660 -2650 5 v -1660 -2650 5 v -1659.24 -2653.83 495 v -1659.24 -2653.83 5 v -1659.24 -2653.83 495 v -1657.07 -2657.07 495 v -1659.24 -2653.83 5 v -1659.24 -2653.83 5 v -1657.07 -2657.07 495 v -1657.07 -2657.07 5 v -1657.07 -2657.07 495 v -1653.83 -2659.24 495 v -1657.07 -2657.07 5 v -1657.07 -2657.07 5 v -1653.83 -2659.24 495 v -1653.83 -2659.24 5 v -1653.83 -2659.24 495 v -1650 -2660 495 v -1653.83 -2659.24 5 v -1653.83 -2659.24 5 v -1650 -2660 495 v -1650 -2660 5 v -1650 -2660 495 v -1646.17 -2659.24 495 v -1650 -2660 5 v -1650 -2660 5 v -1646.17 -2659.24 495 v -1646.17 -2659.24 5 v -1646.17 -2659.24 495 v -1642.93 -2657.07 495 v -1646.17 -2659.24 5 v -1646.17 -2659.24 5 v -1642.93 -2657.07 495 v -1642.93 -2657.07 5 v -1642.93 -2657.07 495 v -1640.76 -2653.83 495 v -1642.93 -2657.07 5 v -1642.93 -2657.07 5 v -1640.76 -2653.83 495 v -1640.76 -2653.83 5 v -1640.76 -2653.83 495 v -1640 -2650 495 v -1640.76 -2653.83 5 v -1640.76 -2653.83 5 v -1640 -2650 495 v -1640 -2650 5 v -1640 -2650 495 v -1640.76 -2646.17 495 v -1640 -2650 5 v -1640 -2650 5 v -1640.76 -2646.17 495 v -1640.76 -2646.17 5 v -1640.76 -2646.17 495 v -1642.93 -2642.93 495 v -1640.76 -2646.17 5 v -1640.76 -2646.17 5 v -1642.93 -2642.93 495 v -1642.93 -2642.93 5 v -1642.93 -2642.93 495 v -1646.17 -2640.76 495 v -1642.93 -2642.93 5 v -1642.93 -2642.93 5 v -1646.17 -2640.76 495 v -1646.17 -2640.76 5 v -1646.17 -2640.76 495 v -1650 -2640 495 v -1646.17 -2640.76 5 v -1646.17 -2640.76 5 v -1650 -2640 495 v -1650 -2640 5 v -1650 -2640 495 v -1653.83 -2640.76 495 v -1650 -2640 5 v -1650 -2640 5 v -1653.83 -2640.76 495 v -1653.83 -2640.76 5 v -1653.83 -2640.76 495 v -1657.07 -2642.93 495 v -1653.83 -2640.76 5 v -1653.83 -2640.76 5 v -1657.07 -2642.93 495 v -1657.07 -2642.93 5 v -1657.07 -2642.93 495 v -1659.24 -2646.17 495 v -1657.07 -2642.93 5 v -1657.07 -2642.93 5 v -1659.24 -2646.17 495 v -1659.24 -2646.17 5 v -1659.24 -2646.17 495 v -1660 -2650 495 v -1659.24 -2646.17 5 v -1659.24 -2646.17 5 v -1660 -2650 495 v -1660 -2650 5 v -1360 -2650 495 v -1359.24 -2653.83 495 v -1360 -2650 5 v -1360 -2650 5 v -1359.24 -2653.83 495 v -1359.24 -2653.83 5 v -1359.24 -2653.83 495 v -1357.07 -2657.07 495 v -1359.24 -2653.83 5 v -1359.24 -2653.83 5 v -1357.07 -2657.07 495 v -1357.07 -2657.07 5 v -1357.07 -2657.07 495 v -1353.83 -2659.24 495 v -1357.07 -2657.07 5 v -1357.07 -2657.07 5 v -1353.83 -2659.24 495 v -1353.83 -2659.24 5 v -1353.83 -2659.24 495 v -1350 -2660 495 v -1353.83 -2659.24 5 v -1353.83 -2659.24 5 v -1350 -2660 495 v -1350 -2660 5 v -1350 -2660 495 v -1346.17 -2659.24 495 v -1350 -2660 5 v -1350 -2660 5 v -1346.17 -2659.24 495 v -1346.17 -2659.24 5 v -1346.17 -2659.24 495 v -1342.93 -2657.07 495 v -1346.17 -2659.24 5 v -1346.17 -2659.24 5 v -1342.93 -2657.07 495 v -1342.93 -2657.07 5 v -1342.93 -2657.07 495 v -1340.76 -2653.83 495 v -1342.93 -2657.07 5 v -1342.93 -2657.07 5 v -1340.76 -2653.83 495 v -1340.76 -2653.83 5 v -1340.76 -2653.83 495 v -1340 -2650 495 v -1340.76 -2653.83 5 v -1340.76 -2653.83 5 v -1340 -2650 495 v -1340 -2650 5 v -1340 -2650 495 v -1340.76 -2646.17 495 v -1340 -2650 5 v -1340 -2650 5 v -1340.76 -2646.17 495 v -1340.76 -2646.17 5 v -1340.76 -2646.17 495 v -1342.93 -2642.93 495 v -1340.76 -2646.17 5 v -1340.76 -2646.17 5 v -1342.93 -2642.93 495 v -1342.93 -2642.93 5 v -1342.93 -2642.93 495 v -1346.17 -2640.76 495 v -1342.93 -2642.93 5 v -1342.93 -2642.93 5 v -1346.17 -2640.76 495 v -1346.17 -2640.76 5 v -1346.17 -2640.76 495 v -1350 -2640 495 v -1346.17 -2640.76 5 v -1346.17 -2640.76 5 v -1350 -2640 495 v -1350 -2640 5 v -1350 -2640 495 v -1353.83 -2640.76 495 v -1350 -2640 5 v -1350 -2640 5 v -1353.83 -2640.76 495 v -1353.83 -2640.76 5 v -1353.83 -2640.76 495 v -1357.07 -2642.93 495 v -1353.83 -2640.76 5 v -1353.83 -2640.76 5 v -1357.07 -2642.93 495 v -1357.07 -2642.93 5 v -1357.07 -2642.93 495 v -1359.24 -2646.17 495 v -1357.07 -2642.93 5 v -1357.07 -2642.93 5 v -1359.24 -2646.17 495 v -1359.24 -2646.17 5 v -1359.24 -2646.17 495 v -1360 -2650 495 v -1359.24 -2646.17 5 v -1359.24 -2646.17 5 v -1360 -2650 495 v -1360 -2650 5 v -1360 -2350 495 v -1359.24 -2353.83 495 v -1360 -2350 5 v -1360 -2350 5 v -1359.24 -2353.83 495 v -1359.24 -2353.83 5 v -1359.24 -2353.83 495 v -1357.07 -2357.07 495 v -1359.24 -2353.83 5 v -1359.24 -2353.83 5 v -1357.07 -2357.07 495 v -1357.07 -2357.07 5 v -1357.07 -2357.07 495 v -1353.83 -2359.24 495 v -1357.07 -2357.07 5 v -1357.07 -2357.07 5 v -1353.83 -2359.24 495 v -1353.83 -2359.24 5 v -1353.83 -2359.24 495 v -1350 -2360 495 v -1353.83 -2359.24 5 v -1353.83 -2359.24 5 v -1350 -2360 495 v -1350 -2360 5 v -1350 -2360 495 v -1346.17 -2359.24 495 v -1350 -2360 5 v -1350 -2360 5 v -1346.17 -2359.24 495 v -1346.17 -2359.24 5 v -1346.17 -2359.24 495 v -1342.93 -2357.07 495 v -1346.17 -2359.24 5 v -1346.17 -2359.24 5 v -1342.93 -2357.07 495 v -1342.93 -2357.07 5 v -1342.93 -2357.07 495 v -1340.76 -2353.83 495 v -1342.93 -2357.07 5 v -1342.93 -2357.07 5 v -1340.76 -2353.83 495 v -1340.76 -2353.83 5 v -1340.76 -2353.83 495 v -1340 -2350 495 v -1340.76 -2353.83 5 v -1340.76 -2353.83 5 v -1340 -2350 495 v -1340 -2350 5 v -1340 -2350 495 v -1340.76 -2346.17 495 v -1340 -2350 5 v -1340 -2350 5 v -1340.76 -2346.17 495 v -1340.76 -2346.17 5 v -1340.76 -2346.17 495 v -1342.93 -2342.93 495 v -1340.76 -2346.17 5 v -1340.76 -2346.17 5 v -1342.93 -2342.93 495 v -1342.93 -2342.93 5 v -1342.93 -2342.93 495 v -1346.17 -2340.76 495 v -1342.93 -2342.93 5 v -1342.93 -2342.93 5 v -1346.17 -2340.76 495 v -1346.17 -2340.76 5 v -1346.17 -2340.76 495 v -1350 -2340 495 v -1346.17 -2340.76 5 v -1346.17 -2340.76 5 v -1350 -2340 495 v -1350 -2340 5 v -1350 -2340 495 v -1353.83 -2340.76 495 v -1350 -2340 5 v -1350 -2340 5 v -1353.83 -2340.76 495 v -1353.83 -2340.76 5 v -1353.83 -2340.76 495 v -1357.07 -2342.93 495 v -1353.83 -2340.76 5 v -1353.83 -2340.76 5 v -1357.07 -2342.93 495 v -1357.07 -2342.93 5 v -1357.07 -2342.93 495 v -1359.24 -2346.17 495 v -1357.07 -2342.93 5 v -1357.07 -2342.93 5 v -1359.24 -2346.17 495 v -1359.24 -2346.17 5 v -1359.24 -2346.17 495 v -1360 -2350 495 v -1359.24 -2346.17 5 v -1359.24 -2346.17 5 v -1360 -2350 495 v -1360 -2350 5 v -1660 -2350 495 v -1659.24 -2353.83 495 v -1660 -2350 5 v -1660 -2350 5 v -1659.24 -2353.83 495 v -1659.24 -2353.83 5 v -1659.24 -2353.83 495 v -1657.07 -2357.07 495 v -1659.24 -2353.83 5 v -1659.24 -2353.83 5 v -1657.07 -2357.07 495 v -1657.07 -2357.07 5 v -1657.07 -2357.07 495 v -1653.83 -2359.24 495 v -1657.07 -2357.07 5 v -1657.07 -2357.07 5 v -1653.83 -2359.24 495 v -1653.83 -2359.24 5 v -1653.83 -2359.24 495 v -1650 -2360 495 v -1653.83 -2359.24 5 v -1653.83 -2359.24 5 v -1650 -2360 495 v -1650 -2360 5 v -1650 -2360 495 v -1646.17 -2359.24 495 v -1650 -2360 5 v -1650 -2360 5 v -1646.17 -2359.24 495 v -1646.17 -2359.24 5 v -1646.17 -2359.24 495 v -1642.93 -2357.07 495 v -1646.17 -2359.24 5 v -1646.17 -2359.24 5 v -1642.93 -2357.07 495 v -1642.93 -2357.07 5 v -1642.93 -2357.07 495 v -1640.76 -2353.83 495 v -1642.93 -2357.07 5 v -1642.93 -2357.07 5 v -1640.76 -2353.83 495 v -1640.76 -2353.83 5 v -1640.76 -2353.83 495 v -1640 -2350 495 v -1640.76 -2353.83 5 v -1640.76 -2353.83 5 v -1640 -2350 495 v -1640 -2350 5 v -1640 -2350 495 v -1640.76 -2346.17 495 v -1640 -2350 5 v -1640 -2350 5 v -1640.76 -2346.17 495 v -1640.76 -2346.17 5 v -1640.76 -2346.17 495 v -1642.93 -2342.93 495 v -1640.76 -2346.17 5 v -1640.76 -2346.17 5 v -1642.93 -2342.93 495 v -1642.93 -2342.93 5 v -1642.93 -2342.93 495 v -1646.17 -2340.76 495 v -1642.93 -2342.93 5 v -1642.93 -2342.93 5 v -1646.17 -2340.76 495 v -1646.17 -2340.76 5 v -1646.17 -2340.76 495 v -1650 -2340 495 v -1646.17 -2340.76 5 v -1646.17 -2340.76 5 v -1650 -2340 495 v -1650 -2340 5 v -1650 -2340 495 v -1653.83 -2340.76 495 v -1650 -2340 5 v -1650 -2340 5 v -1653.83 -2340.76 495 v -1653.83 -2340.76 5 v -1653.83 -2340.76 495 v -1657.07 -2342.93 495 v -1653.83 -2340.76 5 v -1653.83 -2340.76 5 v -1657.07 -2342.93 495 v -1657.07 -2342.93 5 v -1657.07 -2342.93 495 v -1659.24 -2346.17 495 v -1657.07 -2342.93 5 v -1657.07 -2342.93 5 v -1659.24 -2346.17 495 v -1659.24 -2346.17 5 v -1659.24 -2346.17 495 v -1660 -2350 495 v -1659.24 -2346.17 5 v -1659.24 -2346.17 5 v -1660 -2350 495 v -1660 -2350 5 v -1300 -2300 510 v -1700 -2300 510 v -1300 -2700 510 v -1300 -2700 510 v -1700 -2300 510 v -1700 -2700 510 v -1700 -2300 500 v -1300 -2300 500 v -1700 -2700 500 v -1700 -2700 500 v -1300 -2300 500 v -1300 -2700 500 v -1700 -2300 510 v -1700 -2300 500 v -1700 -2700 510 v -1700 -2700 510 v -1700 -2300 500 v -1700 -2700 500 v -1300 -2300 500 v -1300 -2300 510 v -1300 -2700 500 v -1300 -2700 500 v -1300 -2300 510 v -1300 -2700 510 v -1300 -2700 500 v -1300 -2700 510 v -1700 -2700 500 v -1700 -2700 500 v -1300 -2700 510 v -1700 -2700 510 v -1300 -2300 500 v -1700 -2300 500 v -1300 -2300 510 v -1300 -2300 510 v -1700 -2300 500 v -1700 -2300 510 v -1560 -2705 810 v -1559.24 -2708.83 810 v -1560 -2705 510 v -1560 -2705 510 v -1559.24 -2708.83 810 v -1559.24 -2708.83 510 v -1559.24 -2708.83 810 v -1557.07 -2712.07 810 v -1559.24 -2708.83 510 v -1559.24 -2708.83 510 v -1557.07 -2712.07 810 v -1557.07 -2712.07 510 v -1557.07 -2712.07 810 v -1553.83 -2714.24 810 v -1557.07 -2712.07 510 v -1557.07 -2712.07 510 v -1553.83 -2714.24 810 v -1553.83 -2714.24 510 v -1553.83 -2714.24 810 v -1550 -2715 810 v -1553.83 -2714.24 510 v -1553.83 -2714.24 510 v -1550 -2715 810 v -1550 -2715 510 v -1550 -2715 810 v -1546.17 -2714.24 810 v -1550 -2715 510 v -1550 -2715 510 v -1546.17 -2714.24 810 v -1546.17 -2714.24 510 v -1546.17 -2714.24 810 v -1542.93 -2712.07 810 v -1546.17 -2714.24 510 v -1546.17 -2714.24 510 v -1542.93 -2712.07 810 v -1542.93 -2712.07 510 v -1542.93 -2712.07 810 v -1540.76 -2708.83 810 v -1542.93 -2712.07 510 v -1542.93 -2712.07 510 v -1540.76 -2708.83 810 v -1540.76 -2708.83 510 v -1540.76 -2708.83 810 v -1540 -2705 810 v -1540.76 -2708.83 510 v -1540.76 -2708.83 510 v -1540 -2705 810 v -1540 -2705 510 v -1540 -2705 810 v -1540.76 -2701.17 810 v -1540 -2705 510 v -1540 -2705 510 v -1540.76 -2701.17 810 v -1540.76 -2701.17 510 v -1540.76 -2701.17 810 v -1542.93 -2697.93 810 v -1540.76 -2701.17 510 v -1540.76 -2701.17 510 v -1542.93 -2697.93 810 v -1542.93 -2697.93 510 v -1542.93 -2697.93 810 v -1546.17 -2695.76 810 v -1542.93 -2697.93 510 v -1542.93 -2697.93 510 v -1546.17 -2695.76 810 v -1546.17 -2695.76 510 v -1546.17 -2695.76 810 v -1550 -2695 810 v -1546.17 -2695.76 510 v -1546.17 -2695.76 510 v -1550 -2695 810 v -1550 -2695 510 v -1550 -2695 810 v -1553.83 -2695.76 810 v -1550 -2695 510 v -1550 -2695 510 v -1553.83 -2695.76 810 v -1553.83 -2695.76 510 v -1553.83 -2695.76 810 v -1557.07 -2697.93 810 v -1553.83 -2695.76 510 v -1553.83 -2695.76 510 v -1557.07 -2697.93 810 v -1557.07 -2697.93 510 v -1557.07 -2697.93 810 v -1559.24 -2701.17 810 v -1557.07 -2697.93 510 v -1557.07 -2697.93 510 v -1559.24 -2701.17 810 v -1559.24 -2701.17 510 v -1559.24 -2701.17 810 v -1560 -2705 810 v -1559.24 -2701.17 510 v -1559.24 -2701.17 510 v -1560 -2705 810 v -1560 -2705 510 v -1460 -2705 810 v -1459.24 -2708.83 810 v -1460 -2705 510 v -1460 -2705 510 v -1459.24 -2708.83 810 v -1459.24 -2708.83 510 v -1459.24 -2708.83 810 v -1457.07 -2712.07 810 v -1459.24 -2708.83 510 v -1459.24 -2708.83 510 v -1457.07 -2712.07 810 v -1457.07 -2712.07 510 v -1457.07 -2712.07 810 v -1453.83 -2714.24 810 v -1457.07 -2712.07 510 v -1457.07 -2712.07 510 v -1453.83 -2714.24 810 v -1453.83 -2714.24 510 v -1453.83 -2714.24 810 v -1450 -2715 810 v -1453.83 -2714.24 510 v -1453.83 -2714.24 510 v -1450 -2715 810 v -1450 -2715 510 v -1450 -2715 810 v -1446.17 -2714.24 810 v -1450 -2715 510 v -1450 -2715 510 v -1446.17 -2714.24 810 v -1446.17 -2714.24 510 v -1446.17 -2714.24 810 v -1442.93 -2712.07 810 v -1446.17 -2714.24 510 v -1446.17 -2714.24 510 v -1442.93 -2712.07 810 v -1442.93 -2712.07 510 v -1442.93 -2712.07 810 v -1440.76 -2708.83 810 v -1442.93 -2712.07 510 v -1442.93 -2712.07 510 v -1440.76 -2708.83 810 v -1440.76 -2708.83 510 v -1440.76 -2708.83 810 v -1440 -2705 810 v -1440.76 -2708.83 510 v -1440.76 -2708.83 510 v -1440 -2705 810 v -1440 -2705 510 v -1440 -2705 810 v -1440.76 -2701.17 810 v -1440 -2705 510 v -1440 -2705 510 v -1440.76 -2701.17 810 v -1440.76 -2701.17 510 v -1440.76 -2701.17 810 v -1442.93 -2697.93 810 v -1440.76 -2701.17 510 v -1440.76 -2701.17 510 v -1442.93 -2697.93 810 v -1442.93 -2697.93 510 v -1442.93 -2697.93 810 v -1446.17 -2695.76 810 v -1442.93 -2697.93 510 v -1442.93 -2697.93 510 v -1446.17 -2695.76 810 v -1446.17 -2695.76 510 v -1446.17 -2695.76 810 v -1450 -2695 810 v -1446.17 -2695.76 510 v -1446.17 -2695.76 510 v -1450 -2695 810 v -1450 -2695 510 v -1450 -2695 810 v -1453.83 -2695.76 810 v -1450 -2695 510 v -1450 -2695 510 v -1453.83 -2695.76 810 v -1453.83 -2695.76 510 v -1453.83 -2695.76 810 v -1457.07 -2697.93 810 v -1453.83 -2695.76 510 v -1453.83 -2695.76 510 v -1457.07 -2697.93 810 v -1457.07 -2697.93 510 v -1457.07 -2697.93 810 v -1459.24 -2701.17 810 v -1457.07 -2697.93 510 v -1457.07 -2697.93 510 v -1459.24 -2701.17 810 v -1459.24 -2701.17 510 v -1459.24 -2701.17 810 v -1460 -2705 810 v -1459.24 -2701.17 510 v -1459.24 -2701.17 510 v -1460 -2705 810 v -1460 -2705 510 v -1300 -2690 910 v -1700 -2690 910 v -1300 -2700 910 v -1300 -2700 910 v -1700 -2690 910 v -1700 -2700 910 v -1700 -2690 710 v -1300 -2690 710 v -1700 -2700 710 v -1700 -2700 710 v -1300 -2690 710 v -1300 -2700 710 v -1700 -2690 910 v -1700 -2690 710 v -1700 -2700 910 v -1700 -2700 910 v -1700 -2690 710 v -1700 -2700 710 v -1300 -2690 710 v -1300 -2690 910 v -1300 -2700 710 v -1300 -2700 710 v -1300 -2690 910 v -1300 -2700 910 v -1300 -2700 710 v -1300 -2700 910 v -1700 -2700 710 v -1700 -2700 710 v -1300 -2700 910 v -1700 -2700 910 v -1300 -2690 710 v -1700 -2690 710 v -1300 -2690 910 v -1300 -2690 910 v -1700 -2690 710 v -1700 -2690 910 v -798.346 -313.769 495 v -797.753 -309.913 495 v -798.346 -313.769 5 v -798.346 -313.769 5 v -797.753 -309.913 495 v -797.753 -309.913 5 v -797.753 -309.913 495 v -798.68 -306.123 495 v -797.753 -309.913 5 v -797.753 -309.913 5 v -798.68 -306.123 495 v -798.68 -306.123 5 v -798.68 -306.123 495 v -800.987 -302.976 495 v -798.68 -306.123 5 v -798.68 -306.123 5 v -800.987 -302.976 495 v -800.987 -302.976 5 v -800.987 -302.976 495 v -804.323 -300.952 495 v -800.987 -302.976 5 v -800.987 -302.976 5 v -804.323 -300.952 495 v -804.323 -300.952 5 v -804.323 -300.952 495 v -808.179 -300.359 495 v -804.323 -300.952 5 v -804.323 -300.952 5 v -808.179 -300.359 495 v -808.179 -300.359 5 v -808.179 -300.359 495 v -811.969 -301.286 495 v -808.179 -300.359 5 v -808.179 -300.359 5 v -811.969 -301.286 495 v -811.969 -301.286 5 v -811.969 -301.286 495 v -815.116 -303.593 495 v -811.969 -301.286 5 v -811.969 -301.286 5 v -815.116 -303.593 495 v -815.116 -303.593 5 v -815.116 -303.593 495 v -817.14 -306.929 495 v -815.116 -303.593 5 v -815.116 -303.593 5 v -817.14 -306.929 495 v -817.14 -306.929 5 v -817.14 -306.929 495 v -817.734 -310.785 495 v -817.14 -306.929 5 v -817.14 -306.929 5 v -817.734 -310.785 495 v -817.734 -310.785 5 v -817.734 -310.785 495 v -816.806 -314.575 495 v -817.734 -310.785 5 v -817.734 -310.785 5 v -816.806 -314.575 495 v -816.806 -314.575 5 v -816.806 -314.575 495 v -814.499 -317.722 495 v -816.806 -314.575 5 v -816.806 -314.575 5 v -814.499 -317.722 495 v -814.499 -317.722 5 v -814.499 -317.722 495 v -811.163 -319.746 495 v -814.499 -317.722 5 v -814.499 -317.722 5 v -811.163 -319.746 495 v -811.163 -319.746 5 v -811.163 -319.746 495 v -807.307 -320.34 495 v -811.163 -319.746 5 v -811.163 -319.746 5 v -807.307 -320.34 495 v -807.307 -320.34 5 v -807.307 -320.34 495 v -803.517 -319.412 495 v -807.307 -320.34 5 v -807.307 -320.34 5 v -803.517 -319.412 495 v -803.517 -319.412 5 v -803.517 -319.412 495 v -800.37 -317.105 495 v -803.517 -319.412 5 v -803.517 -319.412 5 v -800.37 -317.105 495 v -800.37 -317.105 5 v -800.37 -317.105 495 v -798.346 -313.769 495 v -800.37 -317.105 5 v -800.37 -317.105 5 v -798.346 -313.769 495 v -798.346 -313.769 5 v -1080.25 -211.163 495 v -1079.66 -207.307 495 v -1080.25 -211.163 5 v -1080.25 -211.163 5 v -1079.66 -207.307 495 v -1079.66 -207.307 5 v -1079.66 -207.307 495 v -1080.59 -203.517 495 v -1079.66 -207.307 5 v -1079.66 -207.307 5 v -1080.59 -203.517 495 v -1080.59 -203.517 5 v -1080.59 -203.517 495 v -1082.9 -200.37 495 v -1080.59 -203.517 5 v -1080.59 -203.517 5 v -1082.9 -200.37 495 v -1082.9 -200.37 5 v -1082.9 -200.37 495 v -1086.23 -198.346 495 v -1082.9 -200.37 5 v -1082.9 -200.37 5 v -1086.23 -198.346 495 v -1086.23 -198.346 5 v -1086.23 -198.346 495 v -1090.09 -197.753 495 v -1086.23 -198.346 5 v -1086.23 -198.346 5 v -1090.09 -197.753 495 v -1090.09 -197.753 5 v -1090.09 -197.753 495 v -1093.88 -198.68 495 v -1090.09 -197.753 5 v -1090.09 -197.753 5 v -1093.88 -198.68 495 v -1093.88 -198.68 5 v -1093.88 -198.68 495 v -1097.02 -200.987 495 v -1093.88 -198.68 5 v -1093.88 -198.68 5 v -1097.02 -200.987 495 v -1097.02 -200.987 5 v -1097.02 -200.987 495 v -1099.05 -204.323 495 v -1097.02 -200.987 5 v -1097.02 -200.987 5 v -1099.05 -204.323 495 v -1099.05 -204.323 5 v -1099.05 -204.323 495 v -1099.64 -208.179 495 v -1099.05 -204.323 5 v -1099.05 -204.323 5 v -1099.64 -208.179 495 v -1099.64 -208.179 5 v -1099.64 -208.179 495 v -1098.71 -211.969 495 v -1099.64 -208.179 5 v -1099.64 -208.179 5 v -1098.71 -211.969 495 v -1098.71 -211.969 5 v -1098.71 -211.969 495 v -1096.41 -215.116 495 v -1098.71 -211.969 5 v -1098.71 -211.969 5 v -1096.41 -215.116 495 v -1096.41 -215.116 5 v -1096.41 -215.116 495 v -1093.07 -217.14 495 v -1096.41 -215.116 5 v -1096.41 -215.116 5 v -1093.07 -217.14 495 v -1093.07 -217.14 5 v -1093.07 -217.14 495 v -1089.21 -217.734 495 v -1093.07 -217.14 5 v -1093.07 -217.14 5 v -1089.21 -217.734 495 v -1089.21 -217.734 5 v -1089.21 -217.734 495 v -1085.42 -216.806 495 v -1089.21 -217.734 5 v -1089.21 -217.734 5 v -1085.42 -216.806 495 v -1085.42 -216.806 5 v -1085.42 -216.806 495 v -1082.28 -214.499 495 v -1085.42 -216.806 5 v -1085.42 -216.806 5 v -1082.28 -214.499 495 v -1082.28 -214.499 5 v -1082.28 -214.499 495 v -1080.25 -211.163 495 v -1082.28 -214.499 5 v -1082.28 -214.499 5 v -1080.25 -211.163 495 v -1080.25 -211.163 5 v -1182.86 -493.071 495 v -1182.27 -489.215 495 v -1182.86 -493.071 5 v -1182.86 -493.071 5 v -1182.27 -489.215 495 v -1182.27 -489.215 5 v -1182.27 -489.215 495 v -1183.19 -485.425 495 v -1182.27 -489.215 5 v -1182.27 -489.215 5 v -1183.19 -485.425 495 v -1183.19 -485.425 5 v -1183.19 -485.425 495 v -1185.5 -482.278 495 v -1183.19 -485.425 5 v -1183.19 -485.425 5 v -1185.5 -482.278 495 v -1185.5 -482.278 5 v -1185.5 -482.278 495 v -1188.84 -480.254 495 v -1185.5 -482.278 5 v -1185.5 -482.278 5 v -1188.84 -480.254 495 v -1188.84 -480.254 5 v -1188.84 -480.254 495 v -1192.69 -479.66 495 v -1188.84 -480.254 5 v -1188.84 -480.254 5 v -1192.69 -479.66 495 v -1192.69 -479.66 5 v -1192.69 -479.66 495 v -1196.48 -480.588 495 v -1192.69 -479.66 5 v -1192.69 -479.66 5 v -1196.48 -480.588 495 v -1196.48 -480.588 5 v -1196.48 -480.588 495 v -1199.63 -482.895 495 v -1196.48 -480.588 5 v -1196.48 -480.588 5 v -1199.63 -482.895 495 v -1199.63 -482.895 5 v -1199.63 -482.895 495 v -1201.65 -486.231 495 v -1199.63 -482.895 5 v -1199.63 -482.895 5 v -1201.65 -486.231 495 v -1201.65 -486.231 5 v -1201.65 -486.231 495 v -1202.25 -490.087 495 v -1201.65 -486.231 5 v -1201.65 -486.231 5 v -1202.25 -490.087 495 v -1202.25 -490.087 5 v -1202.25 -490.087 495 v -1201.32 -493.877 495 v -1202.25 -490.087 5 v -1202.25 -490.087 5 v -1201.32 -493.877 495 v -1201.32 -493.877 5 v -1201.32 -493.877 495 v -1199.01 -497.024 495 v -1201.32 -493.877 5 v -1201.32 -493.877 5 v -1199.01 -497.024 495 v -1199.01 -497.024 5 v -1199.01 -497.024 495 v -1195.68 -499.048 495 v -1199.01 -497.024 5 v -1199.01 -497.024 5 v -1195.68 -499.048 495 v -1195.68 -499.048 5 v -1195.68 -499.048 495 v -1191.82 -499.641 495 v -1195.68 -499.048 5 v -1195.68 -499.048 5 v -1191.82 -499.641 495 v -1191.82 -499.641 5 v -1191.82 -499.641 495 v -1188.03 -498.714 495 v -1191.82 -499.641 5 v -1191.82 -499.641 5 v -1188.03 -498.714 495 v -1188.03 -498.714 5 v -1188.03 -498.714 495 v -1184.88 -496.407 495 v -1188.03 -498.714 5 v -1188.03 -498.714 5 v -1184.88 -496.407 495 v -1184.88 -496.407 5 v -1184.88 -496.407 495 v -1182.86 -493.071 495 v -1184.88 -496.407 5 v -1184.88 -496.407 5 v -1182.86 -493.071 495 v -1182.86 -493.071 5 v -900.952 -595.677 495 v -900.359 -591.821 495 v -900.952 -595.677 5 v -900.952 -595.677 5 v -900.359 -591.821 495 v -900.359 -591.821 5 v -900.359 -591.821 495 v -901.286 -588.031 495 v -900.359 -591.821 5 v -900.359 -591.821 5 v -901.286 -588.031 495 v -901.286 -588.031 5 v -901.286 -588.031 495 v -903.593 -584.884 495 v -901.286 -588.031 5 v -901.286 -588.031 5 v -903.593 -584.884 495 v -903.593 -584.884 5 v -903.593 -584.884 495 v -906.929 -582.86 495 v -903.593 -584.884 5 v -903.593 -584.884 5 v -906.929 -582.86 495 v -906.929 -582.86 5 v -906.929 -582.86 495 v -910.785 -582.266 495 v -906.929 -582.86 5 v -906.929 -582.86 5 v -910.785 -582.266 495 v -910.785 -582.266 5 v -910.785 -582.266 495 v -914.575 -583.194 495 v -910.785 -582.266 5 v -910.785 -582.266 5 v -914.575 -583.194 495 v -914.575 -583.194 5 v -914.575 -583.194 495 v -917.722 -585.501 495 v -914.575 -583.194 5 v -914.575 -583.194 5 v -917.722 -585.501 495 v -917.722 -585.501 5 v -917.722 -585.501 495 v -919.746 -588.837 495 v -917.722 -585.501 5 v -917.722 -585.501 5 v -919.746 -588.837 495 v -919.746 -588.837 5 v -919.746 -588.837 495 v -920.34 -592.693 495 v -919.746 -588.837 5 v -919.746 -588.837 5 v -920.34 -592.693 495 v -920.34 -592.693 5 v -920.34 -592.693 495 v -919.412 -596.483 495 v -920.34 -592.693 5 v -920.34 -592.693 5 v -919.412 -596.483 495 v -919.412 -596.483 5 v -919.412 -596.483 495 v -917.105 -599.63 495 v -919.412 -596.483 5 v -919.412 -596.483 5 v -917.105 -599.63 495 v -917.105 -599.63 5 v -917.105 -599.63 495 v -913.769 -601.654 495 v -917.105 -599.63 5 v -917.105 -599.63 5 v -913.769 -601.654 495 v -913.769 -601.654 5 v -913.769 -601.654 495 v -909.913 -602.247 495 v -913.769 -601.654 5 v -913.769 -601.654 5 v -909.913 -602.247 495 v -909.913 -602.247 5 v -909.913 -602.247 495 v -906.123 -601.32 495 v -909.913 -602.247 5 v -909.913 -602.247 5 v -906.123 -601.32 495 v -906.123 -601.32 5 v -906.123 -601.32 495 v -902.976 -599.013 495 v -906.123 -601.32 5 v -906.123 -601.32 5 v -902.976 -599.013 495 v -902.976 -599.013 5 v -902.976 -599.013 495 v -900.952 -595.677 495 v -902.976 -599.013 5 v -902.976 -599.013 5 v -900.952 -595.677 495 v -900.952 -595.677 5 v -1256.34 -519.534 510 v -880.466 -656.343 510 v -1119.53 -143.657 510 v -1119.53 -143.657 510 v -880.466 -656.343 510 v -743.657 -280.466 510 v -880.466 -656.343 500 v -1256.34 -519.534 500 v -743.657 -280.466 500 v -743.657 -280.466 500 v -1256.34 -519.534 500 v -1119.53 -143.657 500 v -880.466 -656.343 510 v -880.466 -656.343 500 v -743.657 -280.466 510 v -743.657 -280.466 510 v -880.466 -656.343 500 v -743.657 -280.466 500 v -1256.34 -519.534 500 v -1256.34 -519.534 510 v -1119.53 -143.657 500 v -1119.53 -143.657 500 v -1256.34 -519.534 510 v -1119.53 -143.657 510 v -1119.53 -143.657 500 v -1119.53 -143.657 510 v -743.657 -280.466 500 v -743.657 -280.466 500 v -1119.53 -143.657 510 v -743.657 -280.466 510 v -1256.34 -519.534 500 v -880.466 -656.343 500 v -1256.34 -519.534 510 v -1256.34 -519.534 510 v -880.466 -656.343 500 v -880.466 -656.343 510 v -873.504 -227.884 810 v -872.911 -224.028 810 v -873.504 -227.884 510 v -873.504 -227.884 510 v -872.911 -224.028 810 v -872.911 -224.028 510 v -872.911 -224.028 810 v -873.838 -220.238 810 v -872.911 -224.028 510 v -872.911 -224.028 510 v -873.838 -220.238 810 v -873.838 -220.238 510 v -873.838 -220.238 810 v -876.145 -217.091 810 v -873.838 -220.238 510 v -873.838 -220.238 510 v -876.145 -217.091 810 v -876.145 -217.091 510 v -876.145 -217.091 810 v -879.481 -215.067 810 v -876.145 -217.091 510 v -876.145 -217.091 510 v -879.481 -215.067 810 v -879.481 -215.067 510 v -879.481 -215.067 810 v -883.337 -214.474 810 v -879.481 -215.067 510 v -879.481 -215.067 510 v -883.337 -214.474 810 v -883.337 -214.474 510 v -883.337 -214.474 810 v -887.127 -215.401 810 v -883.337 -214.474 510 v -883.337 -214.474 510 v -887.127 -215.401 810 v -887.127 -215.401 510 v -887.127 -215.401 810 v -890.274 -217.708 810 v -887.127 -215.401 510 v -887.127 -215.401 510 v -890.274 -217.708 810 v -890.274 -217.708 510 v -890.274 -217.708 810 v -892.298 -221.044 810 v -890.274 -217.708 510 v -890.274 -217.708 510 v -892.298 -221.044 810 v -892.298 -221.044 510 v -892.298 -221.044 810 v -892.892 -224.9 810 v -892.298 -221.044 510 v -892.298 -221.044 510 v -892.892 -224.9 810 v -892.892 -224.9 510 v -892.892 -224.9 810 v -891.964 -228.69 810 v -892.892 -224.9 510 v -892.892 -224.9 510 v -891.964 -228.69 810 v -891.964 -228.69 510 v -891.964 -228.69 810 v -889.657 -231.837 810 v -891.964 -228.69 510 v -891.964 -228.69 510 v -889.657 -231.837 810 v -889.657 -231.837 510 v -889.657 -231.837 810 v -886.321 -233.861 810 v -889.657 -231.837 510 v -889.657 -231.837 510 v -886.321 -233.861 810 v -886.321 -233.861 510 v -886.321 -233.861 810 v -882.465 -234.454 810 v -886.321 -233.861 510 v -886.321 -233.861 510 v -882.465 -234.454 810 v -882.465 -234.454 510 v -882.465 -234.454 810 v -878.675 -233.527 810 v -882.465 -234.454 510 v -882.465 -234.454 510 v -878.675 -233.527 810 v -878.675 -233.527 510 v -878.675 -233.527 810 v -875.528 -231.22 810 v -878.675 -233.527 510 v -878.675 -233.527 510 v -875.528 -231.22 810 v -875.528 -231.22 510 v -875.528 -231.22 810 v -873.504 -227.884 810 v -875.528 -231.22 510 v -875.528 -231.22 510 v -873.504 -227.884 810 v -873.504 -227.884 510 v -967.474 -193.682 810 v -966.88 -189.826 810 v -967.474 -193.682 510 v -967.474 -193.682 510 v -966.88 -189.826 810 v -966.88 -189.826 510 v -966.88 -189.826 810 v -967.807 -186.036 810 v -966.88 -189.826 510 v -966.88 -189.826 510 v -967.807 -186.036 810 v -967.807 -186.036 510 v -967.807 -186.036 810 v -970.115 -182.889 810 v -967.807 -186.036 510 v -967.807 -186.036 510 v -970.115 -182.889 810 v -970.115 -182.889 510 v -970.115 -182.889 810 v -973.45 -180.865 810 v -970.115 -182.889 510 v -970.115 -182.889 510 v -973.45 -180.865 810 v -973.45 -180.865 510 v -973.45 -180.865 810 v -977.307 -180.272 810 v -973.45 -180.865 510 v -973.45 -180.865 510 v -977.307 -180.272 810 v -977.307 -180.272 510 v -977.307 -180.272 810 v -981.097 -181.199 810 v -977.307 -180.272 510 v -977.307 -180.272 510 v -981.097 -181.199 810 v -981.097 -181.199 510 v -981.097 -181.199 810 v -984.243 -183.506 810 v -981.097 -181.199 510 v -981.097 -181.199 510 v -984.243 -183.506 810 v -984.243 -183.506 510 v -984.243 -183.506 810 v -986.267 -186.842 810 v -984.243 -183.506 510 v -984.243 -183.506 510 v -986.267 -186.842 810 v -986.267 -186.842 510 v -986.267 -186.842 810 v -986.861 -190.698 810 v -986.267 -186.842 510 v -986.267 -186.842 510 v -986.861 -190.698 810 v -986.861 -190.698 510 v -986.861 -190.698 810 v -985.934 -194.488 810 v -986.861 -190.698 510 v -986.861 -190.698 510 v -985.934 -194.488 810 v -985.934 -194.488 510 v -985.934 -194.488 810 v -983.626 -197.635 810 v -985.934 -194.488 510 v -985.934 -194.488 510 v -983.626 -197.635 810 v -983.626 -197.635 510 v -983.626 -197.635 810 v -980.291 -199.659 810 v -983.626 -197.635 510 v -983.626 -197.635 510 v -980.291 -199.659 810 v -980.291 -199.659 510 v -980.291 -199.659 810 v -976.434 -200.252 810 v -980.291 -199.659 510 v -980.291 -199.659 510 v -976.434 -200.252 810 v -976.434 -200.252 510 v -976.434 -200.252 810 v -972.644 -199.325 810 v -976.434 -200.252 510 v -976.434 -200.252 510 v -972.644 -199.325 810 v -972.644 -199.325 510 v -972.644 -199.325 810 v -969.498 -197.018 810 v -972.644 -199.325 510 v -972.644 -199.325 510 v -969.498 -197.018 810 v -969.498 -197.018 510 v -969.498 -197.018 810 v -967.474 -193.682 810 v -969.498 -197.018 510 v -969.498 -197.018 510 v -967.474 -193.682 810 v -967.474 -193.682 510 v -1122.95 -153.054 910 v -747.078 -289.862 910 v -1119.53 -143.657 910 v -1119.53 -143.657 910 v -747.078 -289.862 910 v -743.657 -280.465 910 v -747.078 -289.862 710 v -1122.95 -153.054 710 v -743.657 -280.465 710 v -743.657 -280.465 710 v -1122.95 -153.054 710 v -1119.53 -143.657 710 v -747.078 -289.862 910 v -747.078 -289.862 710 v -743.657 -280.465 910 v -743.657 -280.465 910 v -747.078 -289.862 710 v -743.657 -280.465 710 v -1122.95 -153.054 710 v -1122.95 -153.054 910 v -1119.53 -143.657 710 v -1119.53 -143.657 710 v -1122.95 -153.054 910 v -1119.53 -143.657 910 v -1119.53 -143.657 710 v -1119.53 -143.657 910 v -743.657 -280.465 710 v -743.657 -280.465 710 v -1119.53 -143.657 910 v -743.657 -280.465 910 v -1122.95 -153.054 710 v -747.078 -289.862 710 v -1122.95 -153.054 910 v -1122.95 -153.054 910 v -747.078 -289.862 710 v -747.078 -289.862 910 v -1900.95 -1604.32 495 v -1902.98 -1600.99 495 v -1900.95 -1604.32 5 v -1900.95 -1604.32 5 v -1902.98 -1600.99 495 v -1902.98 -1600.99 5 v -1902.98 -1600.99 495 v -1906.12 -1598.68 495 v -1902.98 -1600.99 5 v -1902.98 -1600.99 5 v -1906.12 -1598.68 495 v -1906.12 -1598.68 5 v -1906.12 -1598.68 495 v -1909.91 -1597.75 495 v -1906.12 -1598.68 5 v -1906.12 -1598.68 5 v -1909.91 -1597.75 495 v -1909.91 -1597.75 5 v -1909.91 -1597.75 495 v -1913.77 -1598.35 495 v -1909.91 -1597.75 5 v -1909.91 -1597.75 5 v -1913.77 -1598.35 495 v -1913.77 -1598.35 5 v -1913.77 -1598.35 495 v -1917.1 -1600.37 495 v -1913.77 -1598.35 5 v -1913.77 -1598.35 5 v -1917.1 -1600.37 495 v -1917.1 -1600.37 5 v -1917.1 -1600.37 495 v -1919.41 -1603.52 495 v -1917.1 -1600.37 5 v -1917.1 -1600.37 5 v -1919.41 -1603.52 495 v -1919.41 -1603.52 5 v -1919.41 -1603.52 495 v -1920.34 -1607.31 495 v -1919.41 -1603.52 5 v -1919.41 -1603.52 5 v -1920.34 -1607.31 495 v -1920.34 -1607.31 5 v -1920.34 -1607.31 495 v -1919.75 -1611.16 495 v -1920.34 -1607.31 5 v -1920.34 -1607.31 5 v -1919.75 -1611.16 495 v -1919.75 -1611.16 5 v -1919.75 -1611.16 495 v -1917.72 -1614.5 495 v -1919.75 -1611.16 5 v -1919.75 -1611.16 5 v -1917.72 -1614.5 495 v -1917.72 -1614.5 5 v -1917.72 -1614.5 495 v -1914.58 -1616.81 495 v -1917.72 -1614.5 5 v -1917.72 -1614.5 5 v -1914.58 -1616.81 495 v -1914.58 -1616.81 5 v -1914.58 -1616.81 495 v -1910.79 -1617.73 495 v -1914.58 -1616.81 5 v -1914.58 -1616.81 5 v -1910.79 -1617.73 495 v -1910.79 -1617.73 5 v -1910.79 -1617.73 495 v -1906.93 -1617.14 495 v -1910.79 -1617.73 5 v -1910.79 -1617.73 5 v -1906.93 -1617.14 495 v -1906.93 -1617.14 5 v -1906.93 -1617.14 495 v -1903.59 -1615.12 495 v -1906.93 -1617.14 5 v -1906.93 -1617.14 5 v -1903.59 -1615.12 495 v -1903.59 -1615.12 5 v -1903.59 -1615.12 495 v -1901.29 -1611.97 495 v -1903.59 -1615.12 5 v -1903.59 -1615.12 5 v -1901.29 -1611.97 495 v -1901.29 -1611.97 5 v -1901.29 -1611.97 495 v -1900.36 -1608.18 495 v -1901.29 -1611.97 5 v -1901.29 -1611.97 5 v -1900.36 -1608.18 495 v -1900.36 -1608.18 5 v -1900.36 -1608.18 495 v -1900.95 -1604.32 495 v -1900.36 -1608.18 5 v -1900.36 -1608.18 5 v -1900.95 -1604.32 495 v -1900.95 -1604.32 5 v -2182.86 -1706.93 495 v -2184.88 -1703.59 495 v -2182.86 -1706.93 5 v -2182.86 -1706.93 5 v -2184.88 -1703.59 495 v -2184.88 -1703.59 5 v -2184.88 -1703.59 495 v -2188.03 -1701.29 495 v -2184.88 -1703.59 5 v -2184.88 -1703.59 5 v -2188.03 -1701.29 495 v -2188.03 -1701.29 5 v -2188.03 -1701.29 495 v -2191.82 -1700.36 495 v -2188.03 -1701.29 5 v -2188.03 -1701.29 5 v -2191.82 -1700.36 495 v -2191.82 -1700.36 5 v -2191.82 -1700.36 495 v -2195.68 -1700.95 495 v -2191.82 -1700.36 5 v -2191.82 -1700.36 5 v -2195.68 -1700.95 495 v -2195.68 -1700.95 5 v -2195.68 -1700.95 495 v -2199.01 -1702.98 495 v -2195.68 -1700.95 5 v -2195.68 -1700.95 5 v -2199.01 -1702.98 495 v -2199.01 -1702.98 5 v -2199.01 -1702.98 495 v -2201.32 -1706.12 495 v -2199.01 -1702.98 5 v -2199.01 -1702.98 5 v -2201.32 -1706.12 495 v -2201.32 -1706.12 5 v -2201.32 -1706.12 495 v -2202.25 -1709.91 495 v -2201.32 -1706.12 5 v -2201.32 -1706.12 5 v -2202.25 -1709.91 495 v -2202.25 -1709.91 5 v -2202.25 -1709.91 495 v -2201.65 -1713.77 495 v -2202.25 -1709.91 5 v -2202.25 -1709.91 5 v -2201.65 -1713.77 495 v -2201.65 -1713.77 5 v -2201.65 -1713.77 495 v -2199.63 -1717.1 495 v -2201.65 -1713.77 5 v -2201.65 -1713.77 5 v -2199.63 -1717.1 495 v -2199.63 -1717.1 5 v -2199.63 -1717.1 495 v -2196.48 -1719.41 495 v -2199.63 -1717.1 5 v -2199.63 -1717.1 5 v -2196.48 -1719.41 495 v -2196.48 -1719.41 5 v -2196.48 -1719.41 495 v -2192.69 -1720.34 495 v -2196.48 -1719.41 5 v -2196.48 -1719.41 5 v -2192.69 -1720.34 495 v -2192.69 -1720.34 5 v -2192.69 -1720.34 495 v -2188.84 -1719.75 495 v -2192.69 -1720.34 5 v -2192.69 -1720.34 5 v -2188.84 -1719.75 495 v -2188.84 -1719.75 5 v -2188.84 -1719.75 495 v -2185.5 -1717.72 495 v -2188.84 -1719.75 5 v -2188.84 -1719.75 5 v -2185.5 -1717.72 495 v -2185.5 -1717.72 5 v -2185.5 -1717.72 495 v -2183.19 -1714.58 495 v -2185.5 -1717.72 5 v -2185.5 -1717.72 5 v -2183.19 -1714.58 495 v -2183.19 -1714.58 5 v -2183.19 -1714.58 495 v -2182.27 -1710.79 495 v -2183.19 -1714.58 5 v -2183.19 -1714.58 5 v -2182.27 -1710.79 495 v -2182.27 -1710.79 5 v -2182.27 -1710.79 495 v -2182.86 -1706.93 495 v -2182.27 -1710.79 5 v -2182.27 -1710.79 5 v -2182.86 -1706.93 495 v -2182.86 -1706.93 5 v -2080.25 -1988.84 495 v -2082.28 -1985.5 495 v -2080.25 -1988.84 5 v -2080.25 -1988.84 5 v -2082.28 -1985.5 495 v -2082.28 -1985.5 5 v -2082.28 -1985.5 495 v -2085.42 -1983.19 495 v -2082.28 -1985.5 5 v -2082.28 -1985.5 5 v -2085.42 -1983.19 495 v -2085.42 -1983.19 5 v -2085.42 -1983.19 495 v -2089.21 -1982.27 495 v -2085.42 -1983.19 5 v -2085.42 -1983.19 5 v -2089.21 -1982.27 495 v -2089.21 -1982.27 5 v -2089.21 -1982.27 495 v -2093.07 -1982.86 495 v -2089.21 -1982.27 5 v -2089.21 -1982.27 5 v -2093.07 -1982.86 495 v -2093.07 -1982.86 5 v -2093.07 -1982.86 495 v -2096.41 -1984.88 495 v -2093.07 -1982.86 5 v -2093.07 -1982.86 5 v -2096.41 -1984.88 495 v -2096.41 -1984.88 5 v -2096.41 -1984.88 495 v -2098.71 -1988.03 495 v -2096.41 -1984.88 5 v -2096.41 -1984.88 5 v -2098.71 -1988.03 495 v -2098.71 -1988.03 5 v -2098.71 -1988.03 495 v -2099.64 -1991.82 495 v -2098.71 -1988.03 5 v -2098.71 -1988.03 5 v -2099.64 -1991.82 495 v -2099.64 -1991.82 5 v -2099.64 -1991.82 495 v -2099.05 -1995.68 495 v -2099.64 -1991.82 5 v -2099.64 -1991.82 5 v -2099.05 -1995.68 495 v -2099.05 -1995.68 5 v -2099.05 -1995.68 495 v -2097.02 -1999.01 495 v -2099.05 -1995.68 5 v -2099.05 -1995.68 5 v -2097.02 -1999.01 495 v -2097.02 -1999.01 5 v -2097.02 -1999.01 495 v -2093.88 -2001.32 495 v -2097.02 -1999.01 5 v -2097.02 -1999.01 5 v -2093.88 -2001.32 495 v -2093.88 -2001.32 5 v -2093.88 -2001.32 495 v -2090.09 -2002.25 495 v -2093.88 -2001.32 5 v -2093.88 -2001.32 5 v -2090.09 -2002.25 495 v -2090.09 -2002.25 5 v -2090.09 -2002.25 495 v -2086.23 -2001.65 495 v -2090.09 -2002.25 5 v -2090.09 -2002.25 5 v -2086.23 -2001.65 495 v -2086.23 -2001.65 5 v -2086.23 -2001.65 495 v -2082.9 -1999.63 495 v -2086.23 -2001.65 5 v -2086.23 -2001.65 5 v -2082.9 -1999.63 495 v -2082.9 -1999.63 5 v -2082.9 -1999.63 495 v -2080.59 -1996.48 495 v -2082.9 -1999.63 5 v -2082.9 -1999.63 5 v -2080.59 -1996.48 495 v -2080.59 -1996.48 5 v -2080.59 -1996.48 495 v -2079.66 -1992.69 495 v -2080.59 -1996.48 5 v -2080.59 -1996.48 5 v -2079.66 -1992.69 495 v -2079.66 -1992.69 5 v -2079.66 -1992.69 495 v -2080.25 -1988.84 495 v -2079.66 -1992.69 5 v -2079.66 -1992.69 5 v -2080.25 -1988.84 495 v -2080.25 -1988.84 5 v -1798.35 -1886.23 495 v -1800.37 -1882.9 495 v -1798.35 -1886.23 5 v -1798.35 -1886.23 5 v -1800.37 -1882.9 495 v -1800.37 -1882.9 5 v -1800.37 -1882.9 495 v -1803.52 -1880.59 495 v -1800.37 -1882.9 5 v -1800.37 -1882.9 5 v -1803.52 -1880.59 495 v -1803.52 -1880.59 5 v -1803.52 -1880.59 495 v -1807.31 -1879.66 495 v -1803.52 -1880.59 5 v -1803.52 -1880.59 5 v -1807.31 -1879.66 495 v -1807.31 -1879.66 5 v -1807.31 -1879.66 495 v -1811.16 -1880.25 495 v -1807.31 -1879.66 5 v -1807.31 -1879.66 5 v -1811.16 -1880.25 495 v -1811.16 -1880.25 5 v -1811.16 -1880.25 495 v -1814.5 -1882.28 495 v -1811.16 -1880.25 5 v -1811.16 -1880.25 5 v -1814.5 -1882.28 495 v -1814.5 -1882.28 5 v -1814.5 -1882.28 495 v -1816.81 -1885.42 495 v -1814.5 -1882.28 5 v -1814.5 -1882.28 5 v -1816.81 -1885.42 495 v -1816.81 -1885.42 5 v -1816.81 -1885.42 495 v -1817.73 -1889.21 495 v -1816.81 -1885.42 5 v -1816.81 -1885.42 5 v -1817.73 -1889.21 495 v -1817.73 -1889.21 5 v -1817.73 -1889.21 495 v -1817.14 -1893.07 495 v -1817.73 -1889.21 5 v -1817.73 -1889.21 5 v -1817.14 -1893.07 495 v -1817.14 -1893.07 5 v -1817.14 -1893.07 495 v -1815.12 -1896.41 495 v -1817.14 -1893.07 5 v -1817.14 -1893.07 5 v -1815.12 -1896.41 495 v -1815.12 -1896.41 5 v -1815.12 -1896.41 495 v -1811.97 -1898.71 495 v -1815.12 -1896.41 5 v -1815.12 -1896.41 5 v -1811.97 -1898.71 495 v -1811.97 -1898.71 5 v -1811.97 -1898.71 495 v -1808.18 -1899.64 495 v -1811.97 -1898.71 5 v -1811.97 -1898.71 5 v -1808.18 -1899.64 495 v -1808.18 -1899.64 5 v -1808.18 -1899.64 495 v -1804.32 -1899.05 495 v -1808.18 -1899.64 5 v -1808.18 -1899.64 5 v -1804.32 -1899.05 495 v -1804.32 -1899.05 5 v -1804.32 -1899.05 495 v -1800.99 -1897.02 495 v -1804.32 -1899.05 5 v -1804.32 -1899.05 5 v -1800.99 -1897.02 495 v -1800.99 -1897.02 5 v -1800.99 -1897.02 495 v -1798.68 -1893.88 495 v -1800.99 -1897.02 5 v -1800.99 -1897.02 5 v -1798.68 -1893.88 495 v -1798.68 -1893.88 5 v -1798.68 -1893.88 495 v -1797.75 -1890.09 495 v -1798.68 -1893.88 5 v -1798.68 -1893.88 5 v -1797.75 -1890.09 495 v -1797.75 -1890.09 5 v -1797.75 -1890.09 495 v -1798.35 -1886.23 495 v -1797.75 -1890.09 5 v -1797.75 -1890.09 5 v -1798.35 -1886.23 495 v -1798.35 -1886.23 5 v -2119.53 -2056.34 510 v -1743.66 -1919.53 510 v -2256.34 -1680.47 510 v -2256.34 -1680.47 510 v -1743.66 -1919.53 510 v -1880.47 -1543.66 510 v -1743.66 -1919.53 500 v -2119.53 -2056.34 500 v -1880.47 -1543.66 500 v -1880.47 -1543.66 500 v -2119.53 -2056.34 500 v -2256.34 -1680.47 500 v -1743.66 -1919.53 510 v -1743.66 -1919.53 500 v -1880.47 -1543.66 510 v -1880.47 -1543.66 510 v -1743.66 -1919.53 500 v -1880.47 -1543.66 500 v -2119.53 -2056.34 500 v -2119.53 -2056.34 510 v -2256.34 -1680.47 500 v -2256.34 -1680.47 500 v -2119.53 -2056.34 510 v -2256.34 -1680.47 510 v -2256.34 -1680.47 500 v -2256.34 -1680.47 510 v -1880.47 -1543.66 500 v -1880.47 -1543.66 500 v -2256.34 -1680.47 510 v -1880.47 -1543.66 510 v -2119.53 -2056.34 500 v -1743.66 -1919.53 500 v -2119.53 -2056.34 510 v -2119.53 -2056.34 510 v -1743.66 -1919.53 500 v -1743.66 -1919.53 510 v -2013.73 -1586.84 810 v -2015.76 -1583.51 810 v -2013.73 -1586.84 510 v -2013.73 -1586.84 510 v -2015.76 -1583.51 810 v -2015.76 -1583.51 510 v -2015.76 -1583.51 810 v -2018.9 -1581.2 810 v -2015.76 -1583.51 510 v -2015.76 -1583.51 510 v -2018.9 -1581.2 810 v -2018.9 -1581.2 510 v -2018.9 -1581.2 810 v -2022.69 -1580.27 810 v -2018.9 -1581.2 510 v -2018.9 -1581.2 510 v -2022.69 -1580.27 810 v -2022.69 -1580.27 510 v -2022.69 -1580.27 810 v -2026.55 -1580.86 810 v -2022.69 -1580.27 510 v -2022.69 -1580.27 510 v -2026.55 -1580.86 810 v -2026.55 -1580.86 510 v -2026.55 -1580.86 810 v -2029.89 -1582.89 810 v -2026.55 -1580.86 510 v -2026.55 -1580.86 510 v -2029.89 -1582.89 810 v -2029.89 -1582.89 510 v -2029.89 -1582.89 810 v -2032.19 -1586.04 810 v -2029.89 -1582.89 510 v -2029.89 -1582.89 510 v -2032.19 -1586.04 810 v -2032.19 -1586.04 510 v -2032.19 -1586.04 810 v -2033.12 -1589.83 810 v -2032.19 -1586.04 510 v -2032.19 -1586.04 510 v -2033.12 -1589.83 810 v -2033.12 -1589.83 510 v -2033.12 -1589.83 810 v -2032.53 -1593.68 810 v -2033.12 -1589.83 510 v -2033.12 -1589.83 510 v -2032.53 -1593.68 810 v -2032.53 -1593.68 510 v -2032.53 -1593.68 810 v -2030.5 -1597.02 810 v -2032.53 -1593.68 510 v -2032.53 -1593.68 510 v -2030.5 -1597.02 810 v -2030.5 -1597.02 510 v -2030.5 -1597.02 810 v -2027.36 -1599.33 810 v -2030.5 -1597.02 510 v -2030.5 -1597.02 510 v -2027.36 -1599.33 810 v -2027.36 -1599.33 510 v -2027.36 -1599.33 810 v -2023.57 -1600.25 810 v -2027.36 -1599.33 510 v -2027.36 -1599.33 510 v -2023.57 -1600.25 810 v -2023.57 -1600.25 510 v -2023.57 -1600.25 810 v -2019.71 -1599.66 810 v -2023.57 -1600.25 510 v -2023.57 -1600.25 510 v -2019.71 -1599.66 810 v -2019.71 -1599.66 510 v -2019.71 -1599.66 810 v -2016.37 -1597.63 810 v -2019.71 -1599.66 510 v -2019.71 -1599.66 510 v -2016.37 -1597.63 810 v -2016.37 -1597.63 510 v -2016.37 -1597.63 810 v -2014.07 -1594.49 810 v -2016.37 -1597.63 510 v -2016.37 -1597.63 510 v -2014.07 -1594.49 810 v -2014.07 -1594.49 510 v -2014.07 -1594.49 810 v -2013.14 -1590.7 810 v -2014.07 -1594.49 510 v -2014.07 -1594.49 510 v -2013.14 -1590.7 810 v -2013.14 -1590.7 510 v -2013.14 -1590.7 810 v -2013.73 -1586.84 810 v -2013.14 -1590.7 510 v -2013.14 -1590.7 510 v -2013.73 -1586.84 810 v -2013.73 -1586.84 510 v -2107.7 -1621.04 810 v -2109.73 -1617.71 810 v -2107.7 -1621.04 510 v -2107.7 -1621.04 510 v -2109.73 -1617.71 810 v -2109.73 -1617.71 510 v -2109.73 -1617.71 810 v -2112.87 -1615.4 810 v -2109.73 -1617.71 510 v -2109.73 -1617.71 510 v -2112.87 -1615.4 810 v -2112.87 -1615.4 510 v -2112.87 -1615.4 810 v -2116.66 -1614.47 810 v -2112.87 -1615.4 510 v -2112.87 -1615.4 510 v -2116.66 -1614.47 810 v -2116.66 -1614.47 510 v -2116.66 -1614.47 810 v -2120.52 -1615.07 810 v -2116.66 -1614.47 510 v -2116.66 -1614.47 510 v -2120.52 -1615.07 810 v -2120.52 -1615.07 510 v -2120.52 -1615.07 810 v -2123.85 -1617.09 810 v -2120.52 -1615.07 510 v -2120.52 -1615.07 510 v -2123.85 -1617.09 810 v -2123.85 -1617.09 510 v -2123.85 -1617.09 810 v -2126.16 -1620.24 810 v -2123.85 -1617.09 510 v -2123.85 -1617.09 510 v -2126.16 -1620.24 810 v -2126.16 -1620.24 510 v -2126.16 -1620.24 810 v -2127.09 -1624.03 810 v -2126.16 -1620.24 510 v -2126.16 -1620.24 510 v -2127.09 -1624.03 810 v -2127.09 -1624.03 510 v -2127.09 -1624.03 810 v -2126.5 -1627.88 810 v -2127.09 -1624.03 510 v -2127.09 -1624.03 510 v -2126.5 -1627.88 810 v -2126.5 -1627.88 510 v -2126.5 -1627.88 810 v -2124.47 -1631.22 810 v -2126.5 -1627.88 510 v -2126.5 -1627.88 510 v -2124.47 -1631.22 810 v -2124.47 -1631.22 510 v -2124.47 -1631.22 810 v -2121.32 -1633.53 810 v -2124.47 -1631.22 510 v -2124.47 -1631.22 510 v -2121.32 -1633.53 810 v -2121.32 -1633.53 510 v -2121.32 -1633.53 810 v -2117.53 -1634.45 810 v -2121.32 -1633.53 510 v -2121.32 -1633.53 510 v -2117.53 -1634.45 810 v -2117.53 -1634.45 510 v -2117.53 -1634.45 810 v -2113.68 -1633.86 810 v -2117.53 -1634.45 510 v -2117.53 -1634.45 510 v -2113.68 -1633.86 810 v -2113.68 -1633.86 510 v -2113.68 -1633.86 810 v -2110.34 -1631.84 810 v -2113.68 -1633.86 510 v -2113.68 -1633.86 510 v -2110.34 -1631.84 810 v -2110.34 -1631.84 510 v -2110.34 -1631.84 810 v -2108.04 -1628.69 810 v -2110.34 -1631.84 510 v -2110.34 -1631.84 510 v -2108.04 -1628.69 810 v -2108.04 -1628.69 510 v -2108.04 -1628.69 810 v -2107.11 -1624.9 810 v -2108.04 -1628.69 510 v -2108.04 -1628.69 510 v -2107.11 -1624.9 810 v -2107.11 -1624.9 510 v -2107.11 -1624.9 810 v -2107.7 -1621.04 810 v -2107.11 -1624.9 510 v -2107.11 -1624.9 510 v -2107.7 -1621.04 810 v -2107.7 -1621.04 510 v -2252.92 -1689.86 910 v -1877.05 -1553.05 910 v -2256.34 -1680.47 910 v -2256.34 -1680.47 910 v -1877.05 -1553.05 910 v -1880.47 -1543.66 910 v -1877.05 -1553.05 710 v -2252.92 -1689.86 710 v -1880.47 -1543.66 710 v -1880.47 -1543.66 710 v -2252.92 -1689.86 710 v -2256.34 -1680.47 710 v -1877.05 -1553.05 910 v -1877.05 -1553.05 710 v -1880.47 -1543.66 910 v -1880.47 -1543.66 910 v -1877.05 -1553.05 710 v -1880.47 -1543.66 710 v -2252.92 -1689.86 710 v -2252.92 -1689.86 910 v -2256.34 -1680.47 710 v -2256.34 -1680.47 710 v -2252.92 -1689.86 910 v -2256.34 -1680.47 910 v -2256.34 -1680.47 710 v -2256.34 -1680.47 910 v -1880.47 -1543.66 710 v -1880.47 -1543.66 710 v -2256.34 -1680.47 910 v -1880.47 -1543.66 910 v -2252.92 -1689.86 710 v -1877.05 -1553.05 710 v -2252.92 -1689.86 910 v -2252.92 -1689.86 910 v -1877.05 -1553.05 710 v -1877.05 -1553.05 910 v -2880 1350 695 v -2881.52 1357.65 695 v -2880 1350 5 v -2880 1350 5 v -2881.52 1357.65 695 v -2881.52 1357.65 5 v -2881.52 1357.65 695 v -2885.86 1364.14 695 v -2881.52 1357.65 5 v -2881.52 1357.65 5 v -2885.86 1364.14 695 v -2885.86 1364.14 5 v -2885.86 1364.14 695 v -2892.35 1368.48 695 v -2885.86 1364.14 5 v -2885.86 1364.14 5 v -2892.35 1368.48 695 v -2892.35 1368.48 5 v -2892.35 1368.48 695 v -2900 1370 695 v -2892.35 1368.48 5 v -2892.35 1368.48 5 v -2900 1370 695 v -2900 1370 5 v -2900 1370 695 v -2907.65 1368.48 695 v -2900 1370 5 v -2900 1370 5 v -2907.65 1368.48 695 v -2907.65 1368.48 5 v -2907.65 1368.48 695 v -2914.14 1364.14 695 v -2907.65 1368.48 5 v -2907.65 1368.48 5 v -2914.14 1364.14 695 v -2914.14 1364.14 5 v -2914.14 1364.14 695 v -2918.48 1357.65 695 v -2914.14 1364.14 5 v -2914.14 1364.14 5 v -2918.48 1357.65 695 v -2918.48 1357.65 5 v -2918.48 1357.65 695 v -2920 1350 695 v -2918.48 1357.65 5 v -2918.48 1357.65 5 v -2920 1350 695 v -2920 1350 5 v -2920 1350 695 v -2918.48 1342.35 695 v -2920 1350 5 v -2920 1350 5 v -2918.48 1342.35 695 v -2918.48 1342.35 5 v -2918.48 1342.35 695 v -2914.14 1335.86 695 v -2918.48 1342.35 5 v -2918.48 1342.35 5 v -2914.14 1335.86 695 v -2914.14 1335.86 5 v -2914.14 1335.86 695 v -2907.65 1331.52 695 v -2914.14 1335.86 5 v -2914.14 1335.86 5 v -2907.65 1331.52 695 v -2907.65 1331.52 5 v -2907.65 1331.52 695 v -2900 1330 695 v -2907.65 1331.52 5 v -2907.65 1331.52 5 v -2900 1330 695 v -2900 1330 5 v -2900 1330 695 v -2892.35 1331.52 695 v -2900 1330 5 v -2900 1330 5 v -2892.35 1331.52 695 v -2892.35 1331.52 5 v -2892.35 1331.52 695 v -2885.86 1335.86 695 v -2892.35 1331.52 5 v -2892.35 1331.52 5 v -2885.86 1335.86 695 v -2885.86 1335.86 5 v -2885.86 1335.86 695 v -2881.52 1342.35 695 v -2885.86 1335.86 5 v -2885.86 1335.86 5 v -2881.52 1342.35 695 v -2881.52 1342.35 5 v -2881.52 1342.35 695 v -2880 1350 695 v -2881.52 1342.35 5 v -2881.52 1342.35 5 v -2880 1350 695 v -2880 1350 5 v 20 1350 695 v 18.4776 1357.65 695 v 20 1350 5 v 20 1350 5 v 18.4776 1357.65 695 v 18.4776 1357.65 5 v 18.4776 1357.65 695 v 14.1421 1364.14 695 v 18.4776 1357.65 5 v 18.4776 1357.65 5 v 14.1421 1364.14 695 v 14.1421 1364.14 5 v 14.1421 1364.14 695 v 7.65367 1368.48 695 v 14.1421 1364.14 5 v 14.1421 1364.14 5 v 7.65367 1368.48 695 v 7.65367 1368.48 5 v 7.65367 1368.48 695 v -1e-006 1370 695 v 7.65367 1368.48 5 v 7.65367 1368.48 5 v -1e-006 1370 695 v -1e-006 1370 5 v -1e-006 1370 695 v -7.65367 1368.48 695 v -1e-006 1370 5 v -1e-006 1370 5 v -7.65367 1368.48 695 v -7.65367 1368.48 5 v -7.65367 1368.48 695 v -14.1421 1364.14 695 v -7.65367 1368.48 5 v -7.65367 1368.48 5 v -14.1421 1364.14 695 v -14.1421 1364.14 5 v -14.1421 1364.14 695 v -18.4776 1357.65 695 v -14.1421 1364.14 5 v -14.1421 1364.14 5 v -18.4776 1357.65 695 v -18.4776 1357.65 5 v -18.4776 1357.65 695 v -20 1350 695 v -18.4776 1357.65 5 v -18.4776 1357.65 5 v -20 1350 695 v -20 1350 5 v -20 1350 695 v -18.4776 1342.35 695 v -20 1350 5 v -20 1350 5 v -18.4776 1342.35 695 v -18.4776 1342.35 5 v -18.4776 1342.35 695 v -14.1421 1335.86 695 v -18.4776 1342.35 5 v -18.4776 1342.35 5 v -14.1421 1335.86 695 v -14.1421 1335.86 5 v -14.1421 1335.86 695 v -7.65367 1331.52 695 v -14.1421 1335.86 5 v -14.1421 1335.86 5 v -7.65367 1331.52 695 v -7.65367 1331.52 5 v -7.65367 1331.52 695 v 0 1330 695 v -7.65367 1331.52 5 v -7.65367 1331.52 5 v 0 1330 695 v 0 1330 5 v 0 1330 695 v 7.65367 1331.52 695 v 0 1330 5 v 0 1330 5 v 7.65367 1331.52 695 v 7.65367 1331.52 5 v 7.65367 1331.52 695 v 14.1421 1335.86 695 v 7.65367 1331.52 5 v 7.65367 1331.52 5 v 14.1421 1335.86 695 v 14.1421 1335.86 5 v 14.1421 1335.86 695 v 18.4776 1342.35 695 v 14.1421 1335.86 5 v 14.1421 1335.86 5 v 18.4776 1342.35 695 v 18.4776 1342.35 5 v 18.4776 1342.35 695 v 20 1350 695 v 18.4776 1342.35 5 v 18.4776 1342.35 5 v 20 1350 695 v 20 1350 5 v 20 2650 695 v 18.4776 2657.65 695 v 20 2650 5 v 20 2650 5 v 18.4776 2657.65 695 v 18.4776 2657.65 5 v 18.4776 2657.65 695 v 14.1421 2664.14 695 v 18.4776 2657.65 5 v 18.4776 2657.65 5 v 14.1421 2664.14 695 v 14.1421 2664.14 5 v 14.1421 2664.14 695 v 7.65367 2668.48 695 v 14.1421 2664.14 5 v 14.1421 2664.14 5 v 7.65367 2668.48 695 v 7.65367 2668.48 5 v 7.65367 2668.48 695 v -1e-006 2670 695 v 7.65367 2668.48 5 v 7.65367 2668.48 5 v -1e-006 2670 695 v -1e-006 2670 5 v -1e-006 2670 695 v -7.65367 2668.48 695 v -1e-006 2670 5 v -1e-006 2670 5 v -7.65367 2668.48 695 v -7.65367 2668.48 5 v -7.65367 2668.48 695 v -14.1421 2664.14 695 v -7.65367 2668.48 5 v -7.65367 2668.48 5 v -14.1421 2664.14 695 v -14.1421 2664.14 5 v -14.1421 2664.14 695 v -18.4776 2657.65 695 v -14.1421 2664.14 5 v -14.1421 2664.14 5 v -18.4776 2657.65 695 v -18.4776 2657.65 5 v -18.4776 2657.65 695 v -20 2650 695 v -18.4776 2657.65 5 v -18.4776 2657.65 5 v -20 2650 695 v -20 2650 5 v -20 2650 695 v -18.4776 2642.35 695 v -20 2650 5 v -20 2650 5 v -18.4776 2642.35 695 v -18.4776 2642.35 5 v -18.4776 2642.35 695 v -14.1421 2635.86 695 v -18.4776 2642.35 5 v -18.4776 2642.35 5 v -14.1421 2635.86 695 v -14.1421 2635.86 5 v -14.1421 2635.86 695 v -7.65367 2631.52 695 v -14.1421 2635.86 5 v -14.1421 2635.86 5 v -7.65367 2631.52 695 v -7.65367 2631.52 5 v -7.65367 2631.52 695 v 0 2630 695 v -7.65367 2631.52 5 v -7.65367 2631.52 5 v 0 2630 695 v 0 2630 5 v 0 2630 695 v 7.65367 2631.52 695 v 0 2630 5 v 0 2630 5 v 7.65367 2631.52 695 v 7.65367 2631.52 5 v 7.65367 2631.52 695 v 14.1421 2635.86 695 v 7.65367 2631.52 5 v 7.65367 2631.52 5 v 14.1421 2635.86 695 v 14.1421 2635.86 5 v 14.1421 2635.86 695 v 18.4776 2642.35 695 v 14.1421 2635.86 5 v 14.1421 2635.86 5 v 18.4776 2642.35 695 v 18.4776 2642.35 5 v 18.4776 2642.35 695 v 20 2650 695 v 18.4776 2642.35 5 v 18.4776 2642.35 5 v 20 2650 695 v 20 2650 5 v -2880 2650 695 v -2881.52 2657.65 695 v -2880 2650 5 v -2880 2650 5 v -2881.52 2657.65 695 v -2881.52 2657.65 5 v -2881.52 2657.65 695 v -2885.86 2664.14 695 v -2881.52 2657.65 5 v -2881.52 2657.65 5 v -2885.86 2664.14 695 v -2885.86 2664.14 5 v -2885.86 2664.14 695 v -2892.35 2668.48 695 v -2885.86 2664.14 5 v -2885.86 2664.14 5 v -2892.35 2668.48 695 v -2892.35 2668.48 5 v -2892.35 2668.48 695 v -2900 2670 695 v -2892.35 2668.48 5 v -2892.35 2668.48 5 v -2900 2670 695 v -2900 2670 5 v -2900 2670 695 v -2907.65 2668.48 695 v -2900 2670 5 v -2900 2670 5 v -2907.65 2668.48 695 v -2907.65 2668.48 5 v -2907.65 2668.48 695 v -2914.14 2664.14 695 v -2907.65 2668.48 5 v -2907.65 2668.48 5 v -2914.14 2664.14 695 v -2914.14 2664.14 5 v -2914.14 2664.14 695 v -2918.48 2657.65 695 v -2914.14 2664.14 5 v -2914.14 2664.14 5 v -2918.48 2657.65 695 v -2918.48 2657.65 5 v -2918.48 2657.65 695 v -2920 2650 695 v -2918.48 2657.65 5 v -2918.48 2657.65 5 v -2920 2650 695 v -2920 2650 5 v -2920 2650 695 v -2918.48 2642.35 695 v -2920 2650 5 v -2920 2650 5 v -2918.48 2642.35 695 v -2918.48 2642.35 5 v -2918.48 2642.35 695 v -2914.14 2635.86 695 v -2918.48 2642.35 5 v -2918.48 2642.35 5 v -2914.14 2635.86 695 v -2914.14 2635.86 5 v -2914.14 2635.86 695 v -2907.65 2631.52 695 v -2914.14 2635.86 5 v -2914.14 2635.86 5 v -2907.65 2631.52 695 v -2907.65 2631.52 5 v -2907.65 2631.52 695 v -2900 2630 695 v -2907.65 2631.52 5 v -2907.65 2631.52 5 v -2900 2630 695 v -2900 2630 5 v -2900 2630 695 v -2892.35 2631.52 695 v -2900 2630 5 v -2900 2630 5 v -2892.35 2631.52 695 v -2892.35 2631.52 5 v -2892.35 2631.52 695 v -2885.86 2635.86 695 v -2892.35 2631.52 5 v -2892.35 2631.52 5 v -2885.86 2635.86 695 v -2885.86 2635.86 5 v -2885.86 2635.86 695 v -2881.52 2642.35 695 v -2885.86 2635.86 5 v -2885.86 2635.86 5 v -2881.52 2642.35 695 v -2881.52 2642.35 5 v -2881.52 2642.35 695 v -2880 2650 695 v -2881.52 2642.35 5 v -2881.52 2642.35 5 v -2880 2650 695 v -2880 2650 5 v -2900 1250 730 v -100 1250 730 v -2900 2750 730 v -2900 2750 730 v -100 1250 730 v -100 2750 730 v -100 1250 680 v -2900 1250 680 v -100 2750 680 v -100 2750 680 v -2900 1250 680 v -2900 2750 680 v -100 1250 730 v -100 1250 680 v -100 2750 730 v -100 2750 730 v -100 1250 680 v -100 2750 680 v -2900 1250 680 v -2900 1250 730 v -2900 2750 680 v -2900 2750 680 v -2900 1250 730 v -2900 2750 730 v -2900 2750 680 v -2900 2750 730 v -100 2750 680 v -100 2750 680 v -2900 2750 730 v -100 2750 730 v -2900 1250 680 v -100 1250 680 v -2900 1250 730 v -2900 1250 730 v -100 1250 680 v -100 1250 730 v -281.015 2712.06 495 v -279.138 2715.48 495 v -281.015 2712.06 5 v -281.015 2712.06 5 v -279.138 2715.48 495 v -279.138 2715.48 5 v -279.138 2715.48 495 v -278.713 2719.36 495 v -279.138 2715.48 5 v -279.138 2715.48 5 v -278.713 2719.36 495 v -278.713 2719.36 5 v -278.713 2719.36 495 v -279.805 2723.11 495 v -278.713 2719.36 5 v -278.713 2719.36 5 v -279.805 2723.11 495 v -279.805 2723.11 5 v -279.805 2723.11 495 v -282.247 2726.15 495 v -279.805 2723.11 5 v -279.805 2723.11 5 v -282.247 2726.15 495 v -282.247 2726.15 5 v -282.247 2726.15 495 v -285.668 2728.03 495 v -282.247 2726.15 5 v -282.247 2726.15 5 v -285.668 2728.03 495 v -285.668 2728.03 5 v -285.668 2728.03 495 v -289.547 2728.45 495 v -285.668 2728.03 5 v -285.668 2728.03 5 v -289.547 2728.45 495 v -289.547 2728.45 5 v -289.547 2728.45 495 v -293.293 2727.36 495 v -289.547 2728.45 5 v -289.547 2728.45 5 v -293.293 2727.36 495 v -293.293 2727.36 5 v -293.293 2727.36 495 v -296.336 2724.92 495 v -293.293 2727.36 5 v -293.293 2727.36 5 v -296.336 2724.92 495 v -296.336 2724.92 5 v -296.336 2724.92 495 v -298.212 2721.5 495 v -296.336 2724.92 5 v -296.336 2724.92 5 v -298.212 2721.5 495 v -298.212 2721.5 5 v -298.212 2721.5 495 v -298.637 2717.62 495 v -298.212 2721.5 5 v -298.212 2721.5 5 v -298.637 2717.62 495 v -298.637 2717.62 5 v -298.637 2717.62 495 v -297.545 2713.87 495 v -298.637 2717.62 5 v -298.637 2717.62 5 v -297.545 2713.87 495 v -297.545 2713.87 5 v -297.545 2713.87 495 v -295.103 2710.83 495 v -297.545 2713.87 5 v -297.545 2713.87 5 v -295.103 2710.83 495 v -295.103 2710.83 5 v -295.103 2710.83 495 v -291.682 2708.95 495 v -295.103 2710.83 5 v -295.103 2710.83 5 v -291.682 2708.95 495 v -291.682 2708.95 5 v -291.682 2708.95 495 v -287.804 2708.53 495 v -291.682 2708.95 5 v -291.682 2708.95 5 v -287.804 2708.53 495 v -287.804 2708.53 5 v -287.804 2708.53 495 v -284.058 2709.62 495 v -287.804 2708.53 5 v -287.804 2708.53 5 v -284.058 2709.62 495 v -284.058 2709.62 5 v -284.058 2709.62 495 v -281.015 2712.06 495 v -284.058 2709.62 5 v -284.058 2709.62 5 v -281.015 2712.06 495 v -281.015 2712.06 5 v -510.828 2904.9 495 v -508.951 2908.32 495 v -510.828 2904.9 5 v -510.828 2904.9 5 v -508.951 2908.32 495 v -508.951 2908.32 5 v -508.951 2908.32 495 v -508.527 2912.2 495 v -508.951 2908.32 5 v -508.951 2908.32 5 v -508.527 2912.2 495 v -508.527 2912.2 5 v -508.527 2912.2 495 v -509.618 2915.94 495 v -508.527 2912.2 5 v -508.527 2912.2 5 v -509.618 2915.94 495 v -509.618 2915.94 5 v -509.618 2915.94 495 v -512.061 2918.99 495 v -509.618 2915.94 5 v -509.618 2915.94 5 v -512.061 2918.99 495 v -512.061 2918.99 5 v -512.061 2918.99 495 v -515.481 2920.86 495 v -512.061 2918.99 5 v -512.061 2918.99 5 v -515.481 2920.86 495 v -515.481 2920.86 5 v -515.481 2920.86 495 v -519.36 2921.29 495 v -515.481 2920.86 5 v -515.481 2920.86 5 v -519.36 2921.29 495 v -519.36 2921.29 5 v -519.36 2921.29 495 v -523.106 2920.19 495 v -519.36 2921.29 5 v -519.36 2921.29 5 v -523.106 2920.19 495 v -523.106 2920.19 5 v -523.106 2920.19 495 v -526.149 2917.75 495 v -523.106 2920.19 5 v -523.106 2920.19 5 v -526.149 2917.75 495 v -526.149 2917.75 5 v -526.149 2917.75 495 v -528.026 2914.33 495 v -526.149 2917.75 5 v -526.149 2917.75 5 v -528.026 2914.33 495 v -528.026 2914.33 5 v -528.026 2914.33 495 v -528.451 2910.45 495 v -528.026 2914.33 5 v -528.026 2914.33 5 v -528.451 2910.45 495 v -528.451 2910.45 5 v -528.451 2910.45 495 v -527.359 2906.71 495 v -528.451 2910.45 5 v -528.451 2910.45 5 v -527.359 2906.71 495 v -527.359 2906.71 5 v -527.359 2906.71 495 v -524.916 2903.66 495 v -527.359 2906.71 5 v -527.359 2906.71 5 v -524.916 2903.66 495 v -524.916 2903.66 5 v -524.916 2903.66 495 v -521.496 2901.79 495 v -524.916 2903.66 5 v -524.916 2903.66 5 v -521.496 2901.79 495 v -521.496 2901.79 5 v -521.496 2901.79 495 v -517.617 2901.36 495 v -521.496 2901.79 5 v -521.496 2901.79 5 v -517.617 2901.36 495 v -517.617 2901.36 5 v -517.617 2901.36 495 v -513.871 2902.45 495 v -517.617 2901.36 5 v -517.617 2901.36 5 v -513.871 2902.45 495 v -513.871 2902.45 5 v -513.871 2902.45 495 v -510.828 2904.9 495 v -513.871 2902.45 5 v -513.871 2902.45 5 v -510.828 2904.9 495 v -510.828 2904.9 5 v -703.664 2675.08 495 v -701.788 2678.5 495 v -703.664 2675.08 5 v -703.664 2675.08 5 v -701.788 2678.5 495 v -701.788 2678.5 5 v -701.788 2678.5 495 v -701.363 2682.38 495 v -701.788 2678.5 5 v -701.788 2678.5 5 v -701.363 2682.38 495 v -701.363 2682.38 5 v -701.363 2682.38 495 v -702.455 2686.13 495 v -701.363 2682.38 5 v -701.363 2682.38 5 v -702.455 2686.13 495 v -702.455 2686.13 5 v -702.455 2686.13 495 v -704.897 2689.17 495 v -702.455 2686.13 5 v -702.455 2686.13 5 v -704.897 2689.17 495 v -704.897 2689.17 5 v -704.897 2689.17 495 v -708.318 2691.05 495 v -704.897 2689.17 5 v -704.897 2689.17 5 v -708.318 2691.05 495 v -708.318 2691.05 5 v -708.318 2691.05 495 v -712.196 2691.47 495 v -708.318 2691.05 5 v -708.318 2691.05 5 v -712.196 2691.47 495 v -712.196 2691.47 5 v -712.196 2691.47 495 v -715.942 2690.38 495 v -712.196 2691.47 5 v -712.196 2691.47 5 v -715.942 2690.38 495 v -715.942 2690.38 5 v -715.942 2690.38 495 v -718.985 2687.94 495 v -715.942 2690.38 5 v -715.942 2690.38 5 v -718.985 2687.94 495 v -718.985 2687.94 5 v -718.985 2687.94 495 v -720.862 2684.52 495 v -718.985 2687.94 5 v -718.985 2687.94 5 v -720.862 2684.52 495 v -720.862 2684.52 5 v -720.862 2684.52 495 v -721.287 2680.64 495 v -720.862 2684.52 5 v -720.862 2684.52 5 v -721.287 2680.64 495 v -721.287 2680.64 5 v -721.287 2680.64 495 v -720.195 2676.89 495 v -721.287 2680.64 5 v -721.287 2680.64 5 v -720.195 2676.89 495 v -720.195 2676.89 5 v -720.195 2676.89 495 v -717.753 2673.85 495 v -720.195 2676.89 5 v -720.195 2676.89 5 v -717.753 2673.85 495 v -717.753 2673.85 5 v -717.753 2673.85 495 v -714.332 2671.97 495 v -717.753 2673.85 5 v -717.753 2673.85 5 v -714.332 2671.97 495 v -714.332 2671.97 5 v -714.332 2671.97 495 v -710.453 2671.55 495 v -714.332 2671.97 5 v -714.332 2671.97 5 v -710.453 2671.55 495 v -710.453 2671.55 5 v -710.453 2671.55 495 v -706.707 2672.64 495 v -710.453 2671.55 5 v -710.453 2671.55 5 v -706.707 2672.64 495 v -706.707 2672.64 5 v -706.707 2672.64 495 v -703.664 2675.08 495 v -706.707 2672.64 5 v -706.707 2672.64 5 v -703.664 2675.08 495 v -703.664 2675.08 5 v -473.851 2482.25 495 v -471.974 2485.67 495 v -473.851 2482.25 5 v -473.851 2482.25 5 v -471.974 2485.67 495 v -471.974 2485.67 5 v -471.974 2485.67 495 v -471.55 2489.55 495 v -471.974 2485.67 5 v -471.974 2485.67 5 v -471.55 2489.55 495 v -471.55 2489.55 5 v -471.55 2489.55 495 v -472.641 2493.29 495 v -471.55 2489.55 5 v -471.55 2489.55 5 v -472.641 2493.29 495 v -472.641 2493.29 5 v -472.641 2493.29 495 v -475.084 2496.34 495 v -472.641 2493.29 5 v -472.641 2493.29 5 v -475.084 2496.34 495 v -475.084 2496.34 5 v -475.084 2496.34 495 v -478.504 2498.21 495 v -475.084 2496.34 5 v -475.084 2496.34 5 v -478.504 2498.21 495 v -478.504 2498.21 5 v -478.504 2498.21 495 v -482.383 2498.64 495 v -478.504 2498.21 5 v -478.504 2498.21 5 v -482.383 2498.64 495 v -482.383 2498.64 5 v -482.383 2498.64 495 v -486.129 2497.55 495 v -482.383 2498.64 5 v -482.383 2498.64 5 v -486.129 2497.55 495 v -486.129 2497.55 5 v -486.129 2497.55 495 v -489.172 2495.1 495 v -486.129 2497.55 5 v -486.129 2497.55 5 v -489.172 2495.1 495 v -489.172 2495.1 5 v -489.172 2495.1 495 v -491.049 2491.68 495 v -489.172 2495.1 5 v -489.172 2495.1 5 v -491.049 2491.68 495 v -491.049 2491.68 5 v -491.049 2491.68 495 v -491.473 2487.8 495 v -491.049 2491.68 5 v -491.049 2491.68 5 v -491.473 2487.8 495 v -491.473 2487.8 5 v -491.473 2487.8 495 v -490.382 2484.06 495 v -491.473 2487.8 5 v -491.473 2487.8 5 v -490.382 2484.06 495 v -490.382 2484.06 5 v -490.382 2484.06 495 v -487.939 2481.01 495 v -490.382 2484.06 5 v -490.382 2484.06 5 v -487.939 2481.01 495 v -487.939 2481.01 5 v -487.939 2481.01 495 v -484.519 2479.14 495 v -487.939 2481.01 5 v -487.939 2481.01 5 v -484.519 2479.14 495 v -484.519 2479.14 5 v -484.519 2479.14 495 v -480.64 2478.71 495 v -484.519 2479.14 5 v -484.519 2479.14 5 v -480.64 2478.71 495 v -480.64 2478.71 5 v -480.64 2478.71 495 v -476.894 2479.81 495 v -480.64 2478.71 5 v -480.64 2478.71 5 v -476.894 2479.81 495 v -476.894 2479.81 5 v -476.894 2479.81 495 v -473.851 2482.25 495 v -476.894 2479.81 5 v -476.894 2479.81 5 v -473.851 2482.25 495 v -473.851 2482.25 5 v -781.766 2675.35 510 v -475.349 2418.23 510 v -524.651 2981.77 510 v -524.651 2981.77 510 v -475.349 2418.23 510 v -218.234 2724.65 510 v -475.349 2418.23 500 v -781.766 2675.35 500 v -218.234 2724.65 500 v -218.234 2724.65 500 v -781.766 2675.35 500 v -524.651 2981.77 500 v -475.349 2418.23 510 v -475.349 2418.23 500 v -218.234 2724.65 510 v -218.234 2724.65 510 v -475.349 2418.23 500 v -218.234 2724.65 500 v -781.766 2675.35 500 v -781.766 2675.35 510 v -524.651 2981.77 500 v -524.651 2981.77 500 v -781.766 2675.35 510 v -524.651 2981.77 510 v -524.651 2981.77 500 v -524.651 2981.77 510 v -218.234 2724.65 500 v -218.234 2724.65 500 v -524.651 2981.77 510 v -218.234 2724.65 510 v -781.766 2675.35 500 v -475.349 2418.23 500 v -781.766 2675.35 510 v -781.766 2675.35 510 v -475.349 2418.23 500 v -475.349 2418.23 510 v -322.266 2818.47 810 v -320.389 2821.89 810 v -322.266 2818.47 510 v -322.266 2818.47 510 v -320.389 2821.89 810 v -320.389 2821.89 510 v -320.389 2821.89 810 v -319.964 2825.77 810 v -320.389 2821.89 510 v -320.389 2821.89 510 v -319.964 2825.77 810 v -319.964 2825.77 510 v -319.964 2825.77 810 v -321.056 2829.52 810 v -319.964 2825.77 510 v -319.964 2825.77 510 v -321.056 2829.52 810 v -321.056 2829.52 510 v -321.056 2829.52 810 v -323.498 2832.56 810 v -321.056 2829.52 510 v -321.056 2829.52 510 v -323.498 2832.56 810 v -323.498 2832.56 510 v -323.498 2832.56 810 v -326.919 2834.44 810 v -323.498 2832.56 510 v -323.498 2832.56 510 v -326.919 2834.44 810 v -326.919 2834.44 510 v -326.919 2834.44 810 v -330.798 2834.86 810 v -326.919 2834.44 510 v -326.919 2834.44 510 v -330.798 2834.86 810 v -330.798 2834.86 510 v -330.798 2834.86 810 v -334.544 2833.77 810 v -330.798 2834.86 510 v -330.798 2834.86 510 v -334.544 2833.77 810 v -334.544 2833.77 510 v -334.544 2833.77 810 v -337.587 2831.33 810 v -334.544 2833.77 510 v -334.544 2833.77 510 v -337.587 2831.33 810 v -337.587 2831.33 510 v -337.587 2831.33 810 v -339.464 2827.91 810 v -337.587 2831.33 510 v -337.587 2831.33 510 v -339.464 2827.91 810 v -339.464 2827.91 510 v -339.464 2827.91 810 v -339.888 2824.03 810 v -339.464 2827.91 510 v -339.464 2827.91 510 v -339.888 2824.03 810 v -339.888 2824.03 510 v -339.888 2824.03 810 v -338.796 2820.28 810 v -339.888 2824.03 510 v -339.888 2824.03 510 v -338.796 2820.28 810 v -338.796 2820.28 510 v -338.796 2820.28 810 v -336.354 2817.24 810 v -338.796 2820.28 510 v -338.796 2820.28 510 v -336.354 2817.24 810 v -336.354 2817.24 510 v -336.354 2817.24 810 v -332.933 2815.36 810 v -336.354 2817.24 510 v -336.354 2817.24 510 v -332.933 2815.36 810 v -332.933 2815.36 510 v -332.933 2815.36 810 v -329.055 2814.94 810 v -332.933 2815.36 510 v -332.933 2815.36 510 v -329.055 2814.94 810 v -329.055 2814.94 510 v -329.055 2814.94 810 v -325.309 2816.03 810 v -329.055 2814.94 510 v -329.055 2814.94 510 v -325.309 2816.03 810 v -325.309 2816.03 510 v -325.309 2816.03 810 v -322.266 2818.47 810 v -325.309 2816.03 510 v -325.309 2816.03 510 v -322.266 2818.47 810 v -322.266 2818.47 510 v -398.87 2882.75 810 v -396.994 2886.17 810 v -398.87 2882.75 510 v -398.87 2882.75 510 v -396.994 2886.17 810 v -396.994 2886.17 510 v -396.994 2886.17 810 v -396.569 2890.05 810 v -396.994 2886.17 510 v -396.994 2886.17 510 v -396.569 2890.05 810 v -396.569 2890.05 510 v -396.569 2890.05 810 v -397.661 2893.8 810 v -396.569 2890.05 510 v -396.569 2890.05 510 v -397.661 2893.8 810 v -397.661 2893.8 510 v -397.661 2893.8 810 v -400.103 2896.84 810 v -397.661 2893.8 510 v -397.661 2893.8 510 v -400.103 2896.84 810 v -400.103 2896.84 510 v -400.103 2896.84 810 v -403.524 2898.72 810 v -400.103 2896.84 510 v -400.103 2896.84 510 v -403.524 2898.72 810 v -403.524 2898.72 510 v -403.524 2898.72 810 v -407.402 2899.14 810 v -403.524 2898.72 510 v -403.524 2898.72 510 v -407.402 2899.14 810 v -407.402 2899.14 510 v -407.402 2899.14 810 v -411.148 2898.05 810 v -407.402 2899.14 510 v -407.402 2899.14 510 v -411.148 2898.05 810 v -411.148 2898.05 510 v -411.148 2898.05 810 v -414.191 2895.61 810 v -411.148 2898.05 510 v -411.148 2898.05 510 v -414.191 2895.61 810 v -414.191 2895.61 510 v -414.191 2895.61 810 v -416.068 2892.19 810 v -414.191 2895.61 510 v -414.191 2895.61 510 v -416.068 2892.19 810 v -416.068 2892.19 510 v -416.068 2892.19 810 v -416.493 2888.31 810 v -416.068 2892.19 510 v -416.068 2892.19 510 v -416.493 2888.31 810 v -416.493 2888.31 510 v -416.493 2888.31 810 v -415.401 2884.56 810 v -416.493 2888.31 510 v -416.493 2888.31 510 v -415.401 2884.56 810 v -415.401 2884.56 510 v -415.401 2884.56 810 v -412.959 2881.52 810 v -415.401 2884.56 510 v -415.401 2884.56 510 v -412.959 2881.52 810 v -412.959 2881.52 510 v -412.959 2881.52 810 v -409.538 2879.64 810 v -412.959 2881.52 510 v -412.959 2881.52 510 v -409.538 2879.64 810 v -409.538 2879.64 510 v -409.538 2879.64 810 v -405.659 2879.22 810 v -409.538 2879.64 510 v -409.538 2879.64 510 v -405.659 2879.22 810 v -405.659 2879.22 510 v -405.659 2879.22 810 v -401.913 2880.31 810 v -405.659 2879.22 510 v -405.659 2879.22 510 v -401.913 2880.31 810 v -401.913 2880.31 510 v -401.913 2880.31 810 v -398.87 2882.75 810 v -401.913 2880.31 510 v -401.913 2880.31 510 v -398.87 2882.75 810 v -398.87 2882.75 510 v -531.079 2974.11 910 v -224.661 2716.99 910 v -524.651 2981.77 910 v -524.651 2981.77 910 v -224.661 2716.99 910 v -218.234 2724.65 910 v -224.661 2716.99 710 v -531.079 2974.11 710 v -218.234 2724.65 710 v -218.234 2724.65 710 v -531.079 2974.11 710 v -524.651 2981.77 710 v -224.661 2716.99 910 v -224.661 2716.99 710 v -218.234 2724.65 910 v -218.234 2724.65 910 v -224.661 2716.99 710 v -218.234 2724.65 710 v -531.079 2974.11 710 v -531.079 2974.11 910 v -524.651 2981.77 710 v -524.651 2981.77 710 v -531.079 2974.11 910 v -524.651 2981.77 910 v -524.651 2981.77 710 v -524.651 2981.77 910 v -218.234 2724.65 710 v -218.234 2724.65 710 v -524.651 2981.77 910 v -218.234 2724.65 910 v -531.079 2974.11 710 v -224.661 2716.99 710 v -531.079 2974.11 910 v -531.079 2974.11 910 v -224.661 2716.99 710 v -224.661 2716.99 910 v -1281.01 2712.06 495 v -1279.14 2715.48 495 v -1281.01 2712.06 5 v -1281.01 2712.06 5 v -1279.14 2715.48 495 v -1279.14 2715.48 5 v -1279.14 2715.48 495 v -1278.71 2719.36 495 v -1279.14 2715.48 5 v -1279.14 2715.48 5 v -1278.71 2719.36 495 v -1278.71 2719.36 5 v -1278.71 2719.36 495 v -1279.81 2723.11 495 v -1278.71 2719.36 5 v -1278.71 2719.36 5 v -1279.81 2723.11 495 v -1279.81 2723.11 5 v -1279.81 2723.11 495 v -1282.25 2726.15 495 v -1279.81 2723.11 5 v -1279.81 2723.11 5 v -1282.25 2726.15 495 v -1282.25 2726.15 5 v -1282.25 2726.15 495 v -1285.67 2728.03 495 v -1282.25 2726.15 5 v -1282.25 2726.15 5 v -1285.67 2728.03 495 v -1285.67 2728.03 5 v -1285.67 2728.03 495 v -1289.55 2728.45 495 v -1285.67 2728.03 5 v -1285.67 2728.03 5 v -1289.55 2728.45 495 v -1289.55 2728.45 5 v -1289.55 2728.45 495 v -1293.29 2727.36 495 v -1289.55 2728.45 5 v -1289.55 2728.45 5 v -1293.29 2727.36 495 v -1293.29 2727.36 5 v -1293.29 2727.36 495 v -1296.34 2724.92 495 v -1293.29 2727.36 5 v -1293.29 2727.36 5 v -1296.34 2724.92 495 v -1296.34 2724.92 5 v -1296.34 2724.92 495 v -1298.21 2721.5 495 v -1296.34 2724.92 5 v -1296.34 2724.92 5 v -1298.21 2721.5 495 v -1298.21 2721.5 5 v -1298.21 2721.5 495 v -1298.64 2717.62 495 v -1298.21 2721.5 5 v -1298.21 2721.5 5 v -1298.64 2717.62 495 v -1298.64 2717.62 5 v -1298.64 2717.62 495 v -1297.55 2713.87 495 v -1298.64 2717.62 5 v -1298.64 2717.62 5 v -1297.55 2713.87 495 v -1297.55 2713.87 5 v -1297.55 2713.87 495 v -1295.1 2710.83 495 v -1297.55 2713.87 5 v -1297.55 2713.87 5 v -1295.1 2710.83 495 v -1295.1 2710.83 5 v -1295.1 2710.83 495 v -1291.68 2708.95 495 v -1295.1 2710.83 5 v -1295.1 2710.83 5 v -1291.68 2708.95 495 v -1291.68 2708.95 5 v -1291.68 2708.95 495 v -1287.8 2708.53 495 v -1291.68 2708.95 5 v -1291.68 2708.95 5 v -1287.8 2708.53 495 v -1287.8 2708.53 5 v -1287.8 2708.53 495 v -1284.06 2709.62 495 v -1287.8 2708.53 5 v -1287.8 2708.53 5 v -1284.06 2709.62 495 v -1284.06 2709.62 5 v -1284.06 2709.62 495 v -1281.01 2712.06 495 v -1284.06 2709.62 5 v -1284.06 2709.62 5 v -1281.01 2712.06 495 v -1281.01 2712.06 5 v -1510.83 2904.9 495 v -1508.95 2908.32 495 v -1510.83 2904.9 5 v -1510.83 2904.9 5 v -1508.95 2908.32 495 v -1508.95 2908.32 5 v -1508.95 2908.32 495 v -1508.53 2912.2 495 v -1508.95 2908.32 5 v -1508.95 2908.32 5 v -1508.53 2912.2 495 v -1508.53 2912.2 5 v -1508.53 2912.2 495 v -1509.62 2915.94 495 v -1508.53 2912.2 5 v -1508.53 2912.2 5 v -1509.62 2915.94 495 v -1509.62 2915.94 5 v -1509.62 2915.94 495 v -1512.06 2918.99 495 v -1509.62 2915.94 5 v -1509.62 2915.94 5 v -1512.06 2918.99 495 v -1512.06 2918.99 5 v -1512.06 2918.99 495 v -1515.48 2920.86 495 v -1512.06 2918.99 5 v -1512.06 2918.99 5 v -1515.48 2920.86 495 v -1515.48 2920.86 5 v -1515.48 2920.86 495 v -1519.36 2921.29 495 v -1515.48 2920.86 5 v -1515.48 2920.86 5 v -1519.36 2921.29 495 v -1519.36 2921.29 5 v -1519.36 2921.29 495 v -1523.11 2920.19 495 v -1519.36 2921.29 5 v -1519.36 2921.29 5 v -1523.11 2920.19 495 v -1523.11 2920.19 5 v -1523.11 2920.19 495 v -1526.15 2917.75 495 v -1523.11 2920.19 5 v -1523.11 2920.19 5 v -1526.15 2917.75 495 v -1526.15 2917.75 5 v -1526.15 2917.75 495 v -1528.03 2914.33 495 v -1526.15 2917.75 5 v -1526.15 2917.75 5 v -1528.03 2914.33 495 v -1528.03 2914.33 5 v -1528.03 2914.33 495 v -1528.45 2910.45 495 v -1528.03 2914.33 5 v -1528.03 2914.33 5 v -1528.45 2910.45 495 v -1528.45 2910.45 5 v -1528.45 2910.45 495 v -1527.36 2906.71 495 v -1528.45 2910.45 5 v -1528.45 2910.45 5 v -1527.36 2906.71 495 v -1527.36 2906.71 5 v -1527.36 2906.71 495 v -1524.92 2903.66 495 v -1527.36 2906.71 5 v -1527.36 2906.71 5 v -1524.92 2903.66 495 v -1524.92 2903.66 5 v -1524.92 2903.66 495 v -1521.5 2901.79 495 v -1524.92 2903.66 5 v -1524.92 2903.66 5 v -1521.5 2901.79 495 v -1521.5 2901.79 5 v -1521.5 2901.79 495 v -1517.62 2901.36 495 v -1521.5 2901.79 5 v -1521.5 2901.79 5 v -1517.62 2901.36 495 v -1517.62 2901.36 5 v -1517.62 2901.36 495 v -1513.87 2902.45 495 v -1517.62 2901.36 5 v -1517.62 2901.36 5 v -1513.87 2902.45 495 v -1513.87 2902.45 5 v -1513.87 2902.45 495 v -1510.83 2904.9 495 v -1513.87 2902.45 5 v -1513.87 2902.45 5 v -1510.83 2904.9 495 v -1510.83 2904.9 5 v -1703.66 2675.08 495 v -1701.79 2678.5 495 v -1703.66 2675.08 5 v -1703.66 2675.08 5 v -1701.79 2678.5 495 v -1701.79 2678.5 5 v -1701.79 2678.5 495 v -1701.36 2682.38 495 v -1701.79 2678.5 5 v -1701.79 2678.5 5 v -1701.36 2682.38 495 v -1701.36 2682.38 5 v -1701.36 2682.38 495 v -1702.45 2686.13 495 v -1701.36 2682.38 5 v -1701.36 2682.38 5 v -1702.45 2686.13 495 v -1702.45 2686.13 5 v -1702.45 2686.13 495 v -1704.9 2689.17 495 v -1702.45 2686.13 5 v -1702.45 2686.13 5 v -1704.9 2689.17 495 v -1704.9 2689.17 5 v -1704.9 2689.17 495 v -1708.32 2691.05 495 v -1704.9 2689.17 5 v -1704.9 2689.17 5 v -1708.32 2691.05 495 v -1708.32 2691.05 5 v -1708.32 2691.05 495 v -1712.2 2691.47 495 v -1708.32 2691.05 5 v -1708.32 2691.05 5 v -1712.2 2691.47 495 v -1712.2 2691.47 5 v -1712.2 2691.47 495 v -1715.94 2690.38 495 v -1712.2 2691.47 5 v -1712.2 2691.47 5 v -1715.94 2690.38 495 v -1715.94 2690.38 5 v -1715.94 2690.38 495 v -1718.99 2687.94 495 v -1715.94 2690.38 5 v -1715.94 2690.38 5 v -1718.99 2687.94 495 v -1718.99 2687.94 5 v -1718.99 2687.94 495 v -1720.86 2684.52 495 v -1718.99 2687.94 5 v -1718.99 2687.94 5 v -1720.86 2684.52 495 v -1720.86 2684.52 5 v -1720.86 2684.52 495 v -1721.29 2680.64 495 v -1720.86 2684.52 5 v -1720.86 2684.52 5 v -1721.29 2680.64 495 v -1721.29 2680.64 5 v -1721.29 2680.64 495 v -1720.19 2676.89 495 v -1721.29 2680.64 5 v -1721.29 2680.64 5 v -1720.19 2676.89 495 v -1720.19 2676.89 5 v -1720.19 2676.89 495 v -1717.75 2673.85 495 v -1720.19 2676.89 5 v -1720.19 2676.89 5 v -1717.75 2673.85 495 v -1717.75 2673.85 5 v -1717.75 2673.85 495 v -1714.33 2671.97 495 v -1717.75 2673.85 5 v -1717.75 2673.85 5 v -1714.33 2671.97 495 v -1714.33 2671.97 5 v -1714.33 2671.97 495 v -1710.45 2671.55 495 v -1714.33 2671.97 5 v -1714.33 2671.97 5 v -1710.45 2671.55 495 v -1710.45 2671.55 5 v -1710.45 2671.55 495 v -1706.71 2672.64 495 v -1710.45 2671.55 5 v -1710.45 2671.55 5 v -1706.71 2672.64 495 v -1706.71 2672.64 5 v -1706.71 2672.64 495 v -1703.66 2675.08 495 v -1706.71 2672.64 5 v -1706.71 2672.64 5 v -1703.66 2675.08 495 v -1703.66 2675.08 5 v -1473.85 2482.25 495 v -1471.97 2485.67 495 v -1473.85 2482.25 5 v -1473.85 2482.25 5 v -1471.97 2485.67 495 v -1471.97 2485.67 5 v -1471.97 2485.67 495 v -1471.55 2489.55 495 v -1471.97 2485.67 5 v -1471.97 2485.67 5 v -1471.55 2489.55 495 v -1471.55 2489.55 5 v -1471.55 2489.55 495 v -1472.64 2493.29 495 v -1471.55 2489.55 5 v -1471.55 2489.55 5 v -1472.64 2493.29 495 v -1472.64 2493.29 5 v -1472.64 2493.29 495 v -1475.08 2496.34 495 v -1472.64 2493.29 5 v -1472.64 2493.29 5 v -1475.08 2496.34 495 v -1475.08 2496.34 5 v -1475.08 2496.34 495 v -1478.5 2498.21 495 v -1475.08 2496.34 5 v -1475.08 2496.34 5 v -1478.5 2498.21 495 v -1478.5 2498.21 5 v -1478.5 2498.21 495 v -1482.38 2498.64 495 v -1478.5 2498.21 5 v -1478.5 2498.21 5 v -1482.38 2498.64 495 v -1482.38 2498.64 5 v -1482.38 2498.64 495 v -1486.13 2497.55 495 v -1482.38 2498.64 5 v -1482.38 2498.64 5 v -1486.13 2497.55 495 v -1486.13 2497.55 5 v -1486.13 2497.55 495 v -1489.17 2495.1 495 v -1486.13 2497.55 5 v -1486.13 2497.55 5 v -1489.17 2495.1 495 v -1489.17 2495.1 5 v -1489.17 2495.1 495 v -1491.05 2491.68 495 v -1489.17 2495.1 5 v -1489.17 2495.1 5 v -1491.05 2491.68 495 v -1491.05 2491.68 5 v -1491.05 2491.68 495 v -1491.47 2487.8 495 v -1491.05 2491.68 5 v -1491.05 2491.68 5 v -1491.47 2487.8 495 v -1491.47 2487.8 5 v -1491.47 2487.8 495 v -1490.38 2484.06 495 v -1491.47 2487.8 5 v -1491.47 2487.8 5 v -1490.38 2484.06 495 v -1490.38 2484.06 5 v -1490.38 2484.06 495 v -1487.94 2481.01 495 v -1490.38 2484.06 5 v -1490.38 2484.06 5 v -1487.94 2481.01 495 v -1487.94 2481.01 5 v -1487.94 2481.01 495 v -1484.52 2479.14 495 v -1487.94 2481.01 5 v -1487.94 2481.01 5 v -1484.52 2479.14 495 v -1484.52 2479.14 5 v -1484.52 2479.14 495 v -1480.64 2478.71 495 v -1484.52 2479.14 5 v -1484.52 2479.14 5 v -1480.64 2478.71 495 v -1480.64 2478.71 5 v -1480.64 2478.71 495 v -1476.89 2479.81 495 v -1480.64 2478.71 5 v -1480.64 2478.71 5 v -1476.89 2479.81 495 v -1476.89 2479.81 5 v -1476.89 2479.81 495 v -1473.85 2482.25 495 v -1476.89 2479.81 5 v -1476.89 2479.81 5 v -1473.85 2482.25 495 v -1473.85 2482.25 5 v -1781.77 2675.35 510 v -1475.35 2418.23 510 v -1524.65 2981.77 510 v -1524.65 2981.77 510 v -1475.35 2418.23 510 v -1218.23 2724.65 510 v -1475.35 2418.23 500 v -1781.77 2675.35 500 v -1218.23 2724.65 500 v -1218.23 2724.65 500 v -1781.77 2675.35 500 v -1524.65 2981.77 500 v -1475.35 2418.23 510 v -1475.35 2418.23 500 v -1218.23 2724.65 510 v -1218.23 2724.65 510 v -1475.35 2418.23 500 v -1218.23 2724.65 500 v -1781.77 2675.35 500 v -1781.77 2675.35 510 v -1524.65 2981.77 500 v -1524.65 2981.77 500 v -1781.77 2675.35 510 v -1524.65 2981.77 510 v -1524.65 2981.77 500 v -1524.65 2981.77 510 v -1218.23 2724.65 500 v -1218.23 2724.65 500 v -1524.65 2981.77 510 v -1218.23 2724.65 510 v -1781.77 2675.35 500 v -1475.35 2418.23 500 v -1781.77 2675.35 510 v -1781.77 2675.35 510 v -1475.35 2418.23 500 v -1475.35 2418.23 510 v -1322.27 2818.47 810 v -1320.39 2821.89 810 v -1322.27 2818.47 510 v -1322.27 2818.47 510 v -1320.39 2821.89 810 v -1320.39 2821.89 510 v -1320.39 2821.89 810 v -1319.96 2825.77 810 v -1320.39 2821.89 510 v -1320.39 2821.89 510 v -1319.96 2825.77 810 v -1319.96 2825.77 510 v -1319.96 2825.77 810 v -1321.06 2829.52 810 v -1319.96 2825.77 510 v -1319.96 2825.77 510 v -1321.06 2829.52 810 v -1321.06 2829.52 510 v -1321.06 2829.52 810 v -1323.5 2832.56 810 v -1321.06 2829.52 510 v -1321.06 2829.52 510 v -1323.5 2832.56 810 v -1323.5 2832.56 510 v -1323.5 2832.56 810 v -1326.92 2834.44 810 v -1323.5 2832.56 510 v -1323.5 2832.56 510 v -1326.92 2834.44 810 v -1326.92 2834.44 510 v -1326.92 2834.44 810 v -1330.8 2834.86 810 v -1326.92 2834.44 510 v -1326.92 2834.44 510 v -1330.8 2834.86 810 v -1330.8 2834.86 510 v -1330.8 2834.86 810 v -1334.54 2833.77 810 v -1330.8 2834.86 510 v -1330.8 2834.86 510 v -1334.54 2833.77 810 v -1334.54 2833.77 510 v -1334.54 2833.77 810 v -1337.59 2831.33 810 v -1334.54 2833.77 510 v -1334.54 2833.77 510 v -1337.59 2831.33 810 v -1337.59 2831.33 510 v -1337.59 2831.33 810 v -1339.46 2827.91 810 v -1337.59 2831.33 510 v -1337.59 2831.33 510 v -1339.46 2827.91 810 v -1339.46 2827.91 510 v -1339.46 2827.91 810 v -1339.89 2824.03 810 v -1339.46 2827.91 510 v -1339.46 2827.91 510 v -1339.89 2824.03 810 v -1339.89 2824.03 510 v -1339.89 2824.03 810 v -1338.8 2820.28 810 v -1339.89 2824.03 510 v -1339.89 2824.03 510 v -1338.8 2820.28 810 v -1338.8 2820.28 510 v -1338.8 2820.28 810 v -1336.35 2817.24 810 v -1338.8 2820.28 510 v -1338.8 2820.28 510 v -1336.35 2817.24 810 v -1336.35 2817.24 510 v -1336.35 2817.24 810 v -1332.93 2815.36 810 v -1336.35 2817.24 510 v -1336.35 2817.24 510 v -1332.93 2815.36 810 v -1332.93 2815.36 510 v -1332.93 2815.36 810 v -1329.05 2814.94 810 v -1332.93 2815.36 510 v -1332.93 2815.36 510 v -1329.05 2814.94 810 v -1329.05 2814.94 510 v -1329.05 2814.94 810 v -1325.31 2816.03 810 v -1329.05 2814.94 510 v -1329.05 2814.94 510 v -1325.31 2816.03 810 v -1325.31 2816.03 510 v -1325.31 2816.03 810 v -1322.27 2818.47 810 v -1325.31 2816.03 510 v -1325.31 2816.03 510 v -1322.27 2818.47 810 v -1322.27 2818.47 510 v -1398.87 2882.75 810 v -1396.99 2886.17 810 v -1398.87 2882.75 510 v -1398.87 2882.75 510 v -1396.99 2886.17 810 v -1396.99 2886.17 510 v -1396.99 2886.17 810 v -1396.57 2890.05 810 v -1396.99 2886.17 510 v -1396.99 2886.17 510 v -1396.57 2890.05 810 v -1396.57 2890.05 510 v -1396.57 2890.05 810 v -1397.66 2893.8 810 v -1396.57 2890.05 510 v -1396.57 2890.05 510 v -1397.66 2893.8 810 v -1397.66 2893.8 510 v -1397.66 2893.8 810 v -1400.1 2896.84 810 v -1397.66 2893.8 510 v -1397.66 2893.8 510 v -1400.1 2896.84 810 v -1400.1 2896.84 510 v -1400.1 2896.84 810 v -1403.52 2898.72 810 v -1400.1 2896.84 510 v -1400.1 2896.84 510 v -1403.52 2898.72 810 v -1403.52 2898.72 510 v -1403.52 2898.72 810 v -1407.4 2899.14 810 v -1403.52 2898.72 510 v -1403.52 2898.72 510 v -1407.4 2899.14 810 v -1407.4 2899.14 510 v -1407.4 2899.14 810 v -1411.15 2898.05 810 v -1407.4 2899.14 510 v -1407.4 2899.14 510 v -1411.15 2898.05 810 v -1411.15 2898.05 510 v -1411.15 2898.05 810 v -1414.19 2895.61 810 v -1411.15 2898.05 510 v -1411.15 2898.05 510 v -1414.19 2895.61 810 v -1414.19 2895.61 510 v -1414.19 2895.61 810 v -1416.07 2892.19 810 v -1414.19 2895.61 510 v -1414.19 2895.61 510 v -1416.07 2892.19 810 v -1416.07 2892.19 510 v -1416.07 2892.19 810 v -1416.49 2888.31 810 v -1416.07 2892.19 510 v -1416.07 2892.19 510 v -1416.49 2888.31 810 v -1416.49 2888.31 510 v -1416.49 2888.31 810 v -1415.4 2884.56 810 v -1416.49 2888.31 510 v -1416.49 2888.31 510 v -1415.4 2884.56 810 v -1415.4 2884.56 510 v -1415.4 2884.56 810 v -1412.96 2881.52 810 v -1415.4 2884.56 510 v -1415.4 2884.56 510 v -1412.96 2881.52 810 v -1412.96 2881.52 510 v -1412.96 2881.52 810 v -1409.54 2879.64 810 v -1412.96 2881.52 510 v -1412.96 2881.52 510 v -1409.54 2879.64 810 v -1409.54 2879.64 510 v -1409.54 2879.64 810 v -1405.66 2879.22 810 v -1409.54 2879.64 510 v -1409.54 2879.64 510 v -1405.66 2879.22 810 v -1405.66 2879.22 510 v -1405.66 2879.22 810 v -1401.91 2880.31 810 v -1405.66 2879.22 510 v -1405.66 2879.22 510 v -1401.91 2880.31 810 v -1401.91 2880.31 510 v -1401.91 2880.31 810 v -1398.87 2882.75 810 v -1401.91 2880.31 510 v -1401.91 2880.31 510 v -1398.87 2882.75 810 v -1398.87 2882.75 510 v -1531.08 2974.11 910 v -1224.66 2716.99 910 v -1524.65 2981.77 910 v -1524.65 2981.77 910 v -1224.66 2716.99 910 v -1218.23 2724.65 910 v -1224.66 2716.99 710 v -1531.08 2974.11 710 v -1218.23 2724.65 710 v -1218.23 2724.65 710 v -1531.08 2974.11 710 v -1524.65 2981.77 710 v -1224.66 2716.99 910 v -1224.66 2716.99 710 v -1218.23 2724.65 910 v -1218.23 2724.65 910 v -1224.66 2716.99 710 v -1218.23 2724.65 710 v -1531.08 2974.11 710 v -1531.08 2974.11 910 v -1524.65 2981.77 710 v -1524.65 2981.77 710 v -1531.08 2974.11 910 v -1524.65 2981.77 910 v -1524.65 2981.77 710 v -1524.65 2981.77 910 v -1218.23 2724.65 710 v -1218.23 2724.65 710 v -1524.65 2981.77 910 v -1218.23 2724.65 910 v -1531.08 2974.11 710 v -1224.66 2716.99 710 v -1531.08 2974.11 910 v -1531.08 2974.11 910 v -1224.66 2716.99 710 v -1224.66 2716.99 910 v -2281.01 2712.06 495 v -2279.14 2715.48 495 v -2281.01 2712.06 5 v -2281.01 2712.06 5 v -2279.14 2715.48 495 v -2279.14 2715.48 5 v -2279.14 2715.48 495 v -2278.71 2719.36 495 v -2279.14 2715.48 5 v -2279.14 2715.48 5 v -2278.71 2719.36 495 v -2278.71 2719.36 5 v -2278.71 2719.36 495 v -2279.81 2723.11 495 v -2278.71 2719.36 5 v -2278.71 2719.36 5 v -2279.81 2723.11 495 v -2279.81 2723.11 5 v -2279.81 2723.11 495 v -2282.25 2726.15 495 v -2279.81 2723.11 5 v -2279.81 2723.11 5 v -2282.25 2726.15 495 v -2282.25 2726.15 5 v -2282.25 2726.15 495 v -2285.67 2728.03 495 v -2282.25 2726.15 5 v -2282.25 2726.15 5 v -2285.67 2728.03 495 v -2285.67 2728.03 5 v -2285.67 2728.03 495 v -2289.55 2728.45 495 v -2285.67 2728.03 5 v -2285.67 2728.03 5 v -2289.55 2728.45 495 v -2289.55 2728.45 5 v -2289.55 2728.45 495 v -2293.29 2727.36 495 v -2289.55 2728.45 5 v -2289.55 2728.45 5 v -2293.29 2727.36 495 v -2293.29 2727.36 5 v -2293.29 2727.36 495 v -2296.34 2724.92 495 v -2293.29 2727.36 5 v -2293.29 2727.36 5 v -2296.34 2724.92 495 v -2296.34 2724.92 5 v -2296.34 2724.92 495 v -2298.21 2721.5 495 v -2296.34 2724.92 5 v -2296.34 2724.92 5 v -2298.21 2721.5 495 v -2298.21 2721.5 5 v -2298.21 2721.5 495 v -2298.64 2717.62 495 v -2298.21 2721.5 5 v -2298.21 2721.5 5 v -2298.64 2717.62 495 v -2298.64 2717.62 5 v -2298.64 2717.62 495 v -2297.55 2713.87 495 v -2298.64 2717.62 5 v -2298.64 2717.62 5 v -2297.55 2713.87 495 v -2297.55 2713.87 5 v -2297.55 2713.87 495 v -2295.1 2710.83 495 v -2297.55 2713.87 5 v -2297.55 2713.87 5 v -2295.1 2710.83 495 v -2295.1 2710.83 5 v -2295.1 2710.83 495 v -2291.68 2708.95 495 v -2295.1 2710.83 5 v -2295.1 2710.83 5 v -2291.68 2708.95 495 v -2291.68 2708.95 5 v -2291.68 2708.95 495 v -2287.8 2708.53 495 v -2291.68 2708.95 5 v -2291.68 2708.95 5 v -2287.8 2708.53 495 v -2287.8 2708.53 5 v -2287.8 2708.53 495 v -2284.06 2709.62 495 v -2287.8 2708.53 5 v -2287.8 2708.53 5 v -2284.06 2709.62 495 v -2284.06 2709.62 5 v -2284.06 2709.62 495 v -2281.01 2712.06 495 v -2284.06 2709.62 5 v -2284.06 2709.62 5 v -2281.01 2712.06 495 v -2281.01 2712.06 5 v -2510.83 2904.9 495 v -2508.95 2908.32 495 v -2510.83 2904.9 5 v -2510.83 2904.9 5 v -2508.95 2908.32 495 v -2508.95 2908.32 5 v -2508.95 2908.32 495 v -2508.53 2912.2 495 v -2508.95 2908.32 5 v -2508.95 2908.32 5 v -2508.53 2912.2 495 v -2508.53 2912.2 5 v -2508.53 2912.2 495 v -2509.62 2915.94 495 v -2508.53 2912.2 5 v -2508.53 2912.2 5 v -2509.62 2915.94 495 v -2509.62 2915.94 5 v -2509.62 2915.94 495 v -2512.06 2918.99 495 v -2509.62 2915.94 5 v -2509.62 2915.94 5 v -2512.06 2918.99 495 v -2512.06 2918.99 5 v -2512.06 2918.99 495 v -2515.48 2920.86 495 v -2512.06 2918.99 5 v -2512.06 2918.99 5 v -2515.48 2920.86 495 v -2515.48 2920.86 5 v -2515.48 2920.86 495 v -2519.36 2921.29 495 v -2515.48 2920.86 5 v -2515.48 2920.86 5 v -2519.36 2921.29 495 v -2519.36 2921.29 5 v -2519.36 2921.29 495 v -2523.11 2920.19 495 v -2519.36 2921.29 5 v -2519.36 2921.29 5 v -2523.11 2920.19 495 v -2523.11 2920.19 5 v -2523.11 2920.19 495 v -2526.15 2917.75 495 v -2523.11 2920.19 5 v -2523.11 2920.19 5 v -2526.15 2917.75 495 v -2526.15 2917.75 5 v -2526.15 2917.75 495 v -2528.03 2914.33 495 v -2526.15 2917.75 5 v -2526.15 2917.75 5 v -2528.03 2914.33 495 v -2528.03 2914.33 5 v -2528.03 2914.33 495 v -2528.45 2910.45 495 v -2528.03 2914.33 5 v -2528.03 2914.33 5 v -2528.45 2910.45 495 v -2528.45 2910.45 5 v -2528.45 2910.45 495 v -2527.36 2906.71 495 v -2528.45 2910.45 5 v -2528.45 2910.45 5 v -2527.36 2906.71 495 v -2527.36 2906.71 5 v -2527.36 2906.71 495 v -2524.92 2903.66 495 v -2527.36 2906.71 5 v -2527.36 2906.71 5 v -2524.92 2903.66 495 v -2524.92 2903.66 5 v -2524.92 2903.66 495 v -2521.5 2901.79 495 v -2524.92 2903.66 5 v -2524.92 2903.66 5 v -2521.5 2901.79 495 v -2521.5 2901.79 5 v -2521.5 2901.79 495 v -2517.62 2901.36 495 v -2521.5 2901.79 5 v -2521.5 2901.79 5 v -2517.62 2901.36 495 v -2517.62 2901.36 5 v -2517.62 2901.36 495 v -2513.87 2902.45 495 v -2517.62 2901.36 5 v -2517.62 2901.36 5 v -2513.87 2902.45 495 v -2513.87 2902.45 5 v -2513.87 2902.45 495 v -2510.83 2904.9 495 v -2513.87 2902.45 5 v -2513.87 2902.45 5 v -2510.83 2904.9 495 v -2510.83 2904.9 5 v -2703.66 2675.08 495 v -2701.79 2678.5 495 v -2703.66 2675.08 5 v -2703.66 2675.08 5 v -2701.79 2678.5 495 v -2701.79 2678.5 5 v -2701.79 2678.5 495 v -2701.36 2682.38 495 v -2701.79 2678.5 5 v -2701.79 2678.5 5 v -2701.36 2682.38 495 v -2701.36 2682.38 5 v -2701.36 2682.38 495 v -2702.45 2686.13 495 v -2701.36 2682.38 5 v -2701.36 2682.38 5 v -2702.45 2686.13 495 v -2702.45 2686.13 5 v -2702.45 2686.13 495 v -2704.9 2689.17 495 v -2702.45 2686.13 5 v -2702.45 2686.13 5 v -2704.9 2689.17 495 v -2704.9 2689.17 5 v -2704.9 2689.17 495 v -2708.32 2691.05 495 v -2704.9 2689.17 5 v -2704.9 2689.17 5 v -2708.32 2691.05 495 v -2708.32 2691.05 5 v -2708.32 2691.05 495 v -2712.2 2691.47 495 v -2708.32 2691.05 5 v -2708.32 2691.05 5 v -2712.2 2691.47 495 v -2712.2 2691.47 5 v -2712.2 2691.47 495 v -2715.94 2690.38 495 v -2712.2 2691.47 5 v -2712.2 2691.47 5 v -2715.94 2690.38 495 v -2715.94 2690.38 5 v -2715.94 2690.38 495 v -2718.99 2687.94 495 v -2715.94 2690.38 5 v -2715.94 2690.38 5 v -2718.99 2687.94 495 v -2718.99 2687.94 5 v -2718.99 2687.94 495 v -2720.86 2684.52 495 v -2718.99 2687.94 5 v -2718.99 2687.94 5 v -2720.86 2684.52 495 v -2720.86 2684.52 5 v -2720.86 2684.52 495 v -2721.29 2680.64 495 v -2720.86 2684.52 5 v -2720.86 2684.52 5 v -2721.29 2680.64 495 v -2721.29 2680.64 5 v -2721.29 2680.64 495 v -2720.19 2676.89 495 v -2721.29 2680.64 5 v -2721.29 2680.64 5 v -2720.19 2676.89 495 v -2720.19 2676.89 5 v -2720.19 2676.89 495 v -2717.75 2673.85 495 v -2720.19 2676.89 5 v -2720.19 2676.89 5 v -2717.75 2673.85 495 v -2717.75 2673.85 5 v -2717.75 2673.85 495 v -2714.33 2671.97 495 v -2717.75 2673.85 5 v -2717.75 2673.85 5 v -2714.33 2671.97 495 v -2714.33 2671.97 5 v -2714.33 2671.97 495 v -2710.45 2671.55 495 v -2714.33 2671.97 5 v -2714.33 2671.97 5 v -2710.45 2671.55 495 v -2710.45 2671.55 5 v -2710.45 2671.55 495 v -2706.71 2672.64 495 v -2710.45 2671.55 5 v -2710.45 2671.55 5 v -2706.71 2672.64 495 v -2706.71 2672.64 5 v -2706.71 2672.64 495 v -2703.66 2675.08 495 v -2706.71 2672.64 5 v -2706.71 2672.64 5 v -2703.66 2675.08 495 v -2703.66 2675.08 5 v -2473.85 2482.25 495 v -2471.97 2485.67 495 v -2473.85 2482.25 5 v -2473.85 2482.25 5 v -2471.97 2485.67 495 v -2471.97 2485.67 5 v -2471.97 2485.67 495 v -2471.55 2489.55 495 v -2471.97 2485.67 5 v -2471.97 2485.67 5 v -2471.55 2489.55 495 v -2471.55 2489.55 5 v -2471.55 2489.55 495 v -2472.64 2493.29 495 v -2471.55 2489.55 5 v -2471.55 2489.55 5 v -2472.64 2493.29 495 v -2472.64 2493.29 5 v -2472.64 2493.29 495 v -2475.08 2496.34 495 v -2472.64 2493.29 5 v -2472.64 2493.29 5 v -2475.08 2496.34 495 v -2475.08 2496.34 5 v -2475.08 2496.34 495 v -2478.5 2498.21 495 v -2475.08 2496.34 5 v -2475.08 2496.34 5 v -2478.5 2498.21 495 v -2478.5 2498.21 5 v -2478.5 2498.21 495 v -2482.38 2498.64 495 v -2478.5 2498.21 5 v -2478.5 2498.21 5 v -2482.38 2498.64 495 v -2482.38 2498.64 5 v -2482.38 2498.64 495 v -2486.13 2497.55 495 v -2482.38 2498.64 5 v -2482.38 2498.64 5 v -2486.13 2497.55 495 v -2486.13 2497.55 5 v -2486.13 2497.55 495 v -2489.17 2495.1 495 v -2486.13 2497.55 5 v -2486.13 2497.55 5 v -2489.17 2495.1 495 v -2489.17 2495.1 5 v -2489.17 2495.1 495 v -2491.05 2491.68 495 v -2489.17 2495.1 5 v -2489.17 2495.1 5 v -2491.05 2491.68 495 v -2491.05 2491.68 5 v -2491.05 2491.68 495 v -2491.47 2487.8 495 v -2491.05 2491.68 5 v -2491.05 2491.68 5 v -2491.47 2487.8 495 v -2491.47 2487.8 5 v -2491.47 2487.8 495 v -2490.38 2484.06 495 v -2491.47 2487.8 5 v -2491.47 2487.8 5 v -2490.38 2484.06 495 v -2490.38 2484.06 5 v -2490.38 2484.06 495 v -2487.94 2481.01 495 v -2490.38 2484.06 5 v -2490.38 2484.06 5 v -2487.94 2481.01 495 v -2487.94 2481.01 5 v -2487.94 2481.01 495 v -2484.52 2479.14 495 v -2487.94 2481.01 5 v -2487.94 2481.01 5 v -2484.52 2479.14 495 v -2484.52 2479.14 5 v -2484.52 2479.14 495 v -2480.64 2478.71 495 v -2484.52 2479.14 5 v -2484.52 2479.14 5 v -2480.64 2478.71 495 v -2480.64 2478.71 5 v -2480.64 2478.71 495 v -2476.89 2479.81 495 v -2480.64 2478.71 5 v -2480.64 2478.71 5 v -2476.89 2479.81 495 v -2476.89 2479.81 5 v -2476.89 2479.81 495 v -2473.85 2482.25 495 v -2476.89 2479.81 5 v -2476.89 2479.81 5 v -2473.85 2482.25 495 v -2473.85 2482.25 5 v -2781.77 2675.35 510 v -2475.35 2418.23 510 v -2524.65 2981.77 510 v -2524.65 2981.77 510 v -2475.35 2418.23 510 v -2218.23 2724.65 510 v -2475.35 2418.23 500 v -2781.77 2675.35 500 v -2218.23 2724.65 500 v -2218.23 2724.65 500 v -2781.77 2675.35 500 v -2524.65 2981.77 500 v -2475.35 2418.23 510 v -2475.35 2418.23 500 v -2218.23 2724.65 510 v -2218.23 2724.65 510 v -2475.35 2418.23 500 v -2218.23 2724.65 500 v -2781.77 2675.35 500 v -2781.77 2675.35 510 v -2524.65 2981.77 500 v -2524.65 2981.77 500 v -2781.77 2675.35 510 v -2524.65 2981.77 510 v -2524.65 2981.77 500 v -2524.65 2981.77 510 v -2218.23 2724.65 500 v -2218.23 2724.65 500 v -2524.65 2981.77 510 v -2218.23 2724.65 510 v -2781.77 2675.35 500 v -2475.35 2418.23 500 v -2781.77 2675.35 510 v -2781.77 2675.35 510 v -2475.35 2418.23 500 v -2475.35 2418.23 510 v -2322.27 2818.47 810 v -2320.39 2821.89 810 v -2322.27 2818.47 510 v -2322.27 2818.47 510 v -2320.39 2821.89 810 v -2320.39 2821.89 510 v -2320.39 2821.89 810 v -2319.96 2825.77 810 v -2320.39 2821.89 510 v -2320.39 2821.89 510 v -2319.96 2825.77 810 v -2319.96 2825.77 510 v -2319.96 2825.77 810 v -2321.06 2829.52 810 v -2319.96 2825.77 510 v -2319.96 2825.77 510 v -2321.06 2829.52 810 v -2321.06 2829.52 510 v -2321.06 2829.52 810 v -2323.5 2832.56 810 v -2321.06 2829.52 510 v -2321.06 2829.52 510 v -2323.5 2832.56 810 v -2323.5 2832.56 510 v -2323.5 2832.56 810 v -2326.92 2834.44 810 v -2323.5 2832.56 510 v -2323.5 2832.56 510 v -2326.92 2834.44 810 v -2326.92 2834.44 510 v -2326.92 2834.44 810 v -2330.8 2834.86 810 v -2326.92 2834.44 510 v -2326.92 2834.44 510 v -2330.8 2834.86 810 v -2330.8 2834.86 510 v -2330.8 2834.86 810 v -2334.54 2833.77 810 v -2330.8 2834.86 510 v -2330.8 2834.86 510 v -2334.54 2833.77 810 v -2334.54 2833.77 510 v -2334.54 2833.77 810 v -2337.59 2831.33 810 v -2334.54 2833.77 510 v -2334.54 2833.77 510 v -2337.59 2831.33 810 v -2337.59 2831.33 510 v -2337.59 2831.33 810 v -2339.46 2827.91 810 v -2337.59 2831.33 510 v -2337.59 2831.33 510 v -2339.46 2827.91 810 v -2339.46 2827.91 510 v -2339.46 2827.91 810 v -2339.89 2824.03 810 v -2339.46 2827.91 510 v -2339.46 2827.91 510 v -2339.89 2824.03 810 v -2339.89 2824.03 510 v -2339.89 2824.03 810 v -2338.8 2820.28 810 v -2339.89 2824.03 510 v -2339.89 2824.03 510 v -2338.8 2820.28 810 v -2338.8 2820.28 510 v -2338.8 2820.28 810 v -2336.35 2817.24 810 v -2338.8 2820.28 510 v -2338.8 2820.28 510 v -2336.35 2817.24 810 v -2336.35 2817.24 510 v -2336.35 2817.24 810 v -2332.93 2815.36 810 v -2336.35 2817.24 510 v -2336.35 2817.24 510 v -2332.93 2815.36 810 v -2332.93 2815.36 510 v -2332.93 2815.36 810 v -2329.05 2814.94 810 v -2332.93 2815.36 510 v -2332.93 2815.36 510 v -2329.05 2814.94 810 v -2329.05 2814.94 510 v -2329.05 2814.94 810 v -2325.31 2816.03 810 v -2329.05 2814.94 510 v -2329.05 2814.94 510 v -2325.31 2816.03 810 v -2325.31 2816.03 510 v -2325.31 2816.03 810 v -2322.27 2818.47 810 v -2325.31 2816.03 510 v -2325.31 2816.03 510 v -2322.27 2818.47 810 v -2322.27 2818.47 510 v -2398.87 2882.75 810 v -2396.99 2886.17 810 v -2398.87 2882.75 510 v -2398.87 2882.75 510 v -2396.99 2886.17 810 v -2396.99 2886.17 510 v -2396.99 2886.17 810 v -2396.57 2890.05 810 v -2396.99 2886.17 510 v -2396.99 2886.17 510 v -2396.57 2890.05 810 v -2396.57 2890.05 510 v -2396.57 2890.05 810 v -2397.66 2893.8 810 v -2396.57 2890.05 510 v -2396.57 2890.05 510 v -2397.66 2893.8 810 v -2397.66 2893.8 510 v -2397.66 2893.8 810 v -2400.1 2896.84 810 v -2397.66 2893.8 510 v -2397.66 2893.8 510 v -2400.1 2896.84 810 v -2400.1 2896.84 510 v -2400.1 2896.84 810 v -2403.52 2898.72 810 v -2400.1 2896.84 510 v -2400.1 2896.84 510 v -2403.52 2898.72 810 v -2403.52 2898.72 510 v -2403.52 2898.72 810 v -2407.4 2899.14 810 v -2403.52 2898.72 510 v -2403.52 2898.72 510 v -2407.4 2899.14 810 v -2407.4 2899.14 510 v -2407.4 2899.14 810 v -2411.15 2898.05 810 v -2407.4 2899.14 510 v -2407.4 2899.14 510 v -2411.15 2898.05 810 v -2411.15 2898.05 510 v -2411.15 2898.05 810 v -2414.19 2895.61 810 v -2411.15 2898.05 510 v -2411.15 2898.05 510 v -2414.19 2895.61 810 v -2414.19 2895.61 510 v -2414.19 2895.61 810 v -2416.07 2892.19 810 v -2414.19 2895.61 510 v -2414.19 2895.61 510 v -2416.07 2892.19 810 v -2416.07 2892.19 510 v -2416.07 2892.19 810 v -2416.49 2888.31 810 v -2416.07 2892.19 510 v -2416.07 2892.19 510 v -2416.49 2888.31 810 v -2416.49 2888.31 510 v -2416.49 2888.31 810 v -2415.4 2884.56 810 v -2416.49 2888.31 510 v -2416.49 2888.31 510 v -2415.4 2884.56 810 v -2415.4 2884.56 510 v -2415.4 2884.56 810 v -2412.96 2881.52 810 v -2415.4 2884.56 510 v -2415.4 2884.56 510 v -2412.96 2881.52 810 v -2412.96 2881.52 510 v -2412.96 2881.52 810 v -2409.54 2879.64 810 v -2412.96 2881.52 510 v -2412.96 2881.52 510 v -2409.54 2879.64 810 v -2409.54 2879.64 510 v -2409.54 2879.64 810 v -2405.66 2879.22 810 v -2409.54 2879.64 510 v -2409.54 2879.64 510 v -2405.66 2879.22 810 v -2405.66 2879.22 510 v -2405.66 2879.22 810 v -2401.91 2880.31 810 v -2405.66 2879.22 510 v -2405.66 2879.22 510 v -2401.91 2880.31 810 v -2401.91 2880.31 510 v -2401.91 2880.31 810 v -2398.87 2882.75 810 v -2401.91 2880.31 510 v -2401.91 2880.31 510 v -2398.87 2882.75 810 v -2398.87 2882.75 510 v -2531.08 2974.11 910 v -2224.66 2716.99 910 v -2524.65 2981.77 910 v -2524.65 2981.77 910 v -2224.66 2716.99 910 v -2218.23 2724.65 910 v -2224.66 2716.99 710 v -2531.08 2974.11 710 v -2218.23 2724.65 710 v -2218.23 2724.65 710 v -2531.08 2974.11 710 v -2524.65 2981.77 710 v -2224.66 2716.99 910 v -2224.66 2716.99 710 v -2218.23 2724.65 910 v -2218.23 2724.65 910 v -2224.66 2716.99 710 v -2218.23 2724.65 710 v -2531.08 2974.11 710 v -2531.08 2974.11 910 v -2524.65 2981.77 710 v -2524.65 2981.77 710 v -2531.08 2974.11 910 v -2524.65 2981.77 910 v -2524.65 2981.77 710 v -2524.65 2981.77 910 v -2218.23 2724.65 710 v -2218.23 2724.65 710 v -2524.65 2981.77 910 v -2218.23 2724.65 910 v -2531.08 2974.11 710 v -2224.66 2716.99 710 v -2531.08 2974.11 910 v -2531.08 2974.11 910 v -2224.66 2716.99 710 v -2224.66 2716.99 910 v -660 1150 495 v -659.239 1146.17 495 v -660 1150 5 v -660 1150 5 v -659.239 1146.17 495 v -659.239 1146.17 5 v -659.239 1146.17 495 v -657.071 1142.93 495 v -659.239 1146.17 5 v -659.239 1146.17 5 v -657.071 1142.93 495 v -657.071 1142.93 5 v -657.071 1142.93 495 v -653.827 1140.76 495 v -657.071 1142.93 5 v -657.071 1142.93 5 v -653.827 1140.76 495 v -653.827 1140.76 5 v -653.827 1140.76 495 v -650 1140 495 v -653.827 1140.76 5 v -653.827 1140.76 5 v -650 1140 495 v -650 1140 5 v -650 1140 495 v -646.173 1140.76 495 v -650 1140 5 v -650 1140 5 v -646.173 1140.76 495 v -646.173 1140.76 5 v -646.173 1140.76 495 v -642.929 1142.93 495 v -646.173 1140.76 5 v -646.173 1140.76 5 v -642.929 1142.93 495 v -642.929 1142.93 5 v -642.929 1142.93 495 v -640.761 1146.17 495 v -642.929 1142.93 5 v -642.929 1142.93 5 v -640.761 1146.17 495 v -640.761 1146.17 5 v -640.761 1146.17 495 v -640 1150 495 v -640.761 1146.17 5 v -640.761 1146.17 5 v -640 1150 495 v -640 1150 5 v -640 1150 495 v -640.761 1153.83 495 v -640 1150 5 v -640 1150 5 v -640.761 1153.83 495 v -640.761 1153.83 5 v -640.761 1153.83 495 v -642.929 1157.07 495 v -640.761 1153.83 5 v -640.761 1153.83 5 v -642.929 1157.07 495 v -642.929 1157.07 5 v -642.929 1157.07 495 v -646.173 1159.24 495 v -642.929 1157.07 5 v -642.929 1157.07 5 v -646.173 1159.24 495 v -646.173 1159.24 5 v -646.173 1159.24 495 v -650 1160 495 v -646.173 1159.24 5 v -646.173 1159.24 5 v -650 1160 495 v -650 1160 5 v -650 1160 495 v -653.827 1159.24 495 v -650 1160 5 v -650 1160 5 v -653.827 1159.24 495 v -653.827 1159.24 5 v -653.827 1159.24 495 v -657.071 1157.07 495 v -653.827 1159.24 5 v -653.827 1159.24 5 v -657.071 1157.07 495 v -657.071 1157.07 5 v -657.071 1157.07 495 v -659.239 1153.83 495 v -657.071 1157.07 5 v -657.071 1157.07 5 v -659.239 1153.83 495 v -659.239 1153.83 5 v -659.239 1153.83 495 v -660 1150 495 v -659.239 1153.83 5 v -659.239 1153.83 5 v -660 1150 495 v -660 1150 5 v -360 1150 495 v -359.239 1146.17 495 v -360 1150 5 v -360 1150 5 v -359.239 1146.17 495 v -359.239 1146.17 5 v -359.239 1146.17 495 v -357.071 1142.93 495 v -359.239 1146.17 5 v -359.239 1146.17 5 v -357.071 1142.93 495 v -357.071 1142.93 5 v -357.071 1142.93 495 v -353.827 1140.76 495 v -357.071 1142.93 5 v -357.071 1142.93 5 v -353.827 1140.76 495 v -353.827 1140.76 5 v -353.827 1140.76 495 v -350 1140 495 v -353.827 1140.76 5 v -353.827 1140.76 5 v -350 1140 495 v -350 1140 5 v -350 1140 495 v -346.173 1140.76 495 v -350 1140 5 v -350 1140 5 v -346.173 1140.76 495 v -346.173 1140.76 5 v -346.173 1140.76 495 v -342.929 1142.93 495 v -346.173 1140.76 5 v -346.173 1140.76 5 v -342.929 1142.93 495 v -342.929 1142.93 5 v -342.929 1142.93 495 v -340.761 1146.17 495 v -342.929 1142.93 5 v -342.929 1142.93 5 v -340.761 1146.17 495 v -340.761 1146.17 5 v -340.761 1146.17 495 v -340 1150 495 v -340.761 1146.17 5 v -340.761 1146.17 5 v -340 1150 495 v -340 1150 5 v -340 1150 495 v -340.761 1153.83 495 v -340 1150 5 v -340 1150 5 v -340.761 1153.83 495 v -340.761 1153.83 5 v -340.761 1153.83 495 v -342.929 1157.07 495 v -340.761 1153.83 5 v -340.761 1153.83 5 v -342.929 1157.07 495 v -342.929 1157.07 5 v -342.929 1157.07 495 v -346.173 1159.24 495 v -342.929 1157.07 5 v -342.929 1157.07 5 v -346.173 1159.24 495 v -346.173 1159.24 5 v -346.173 1159.24 495 v -350 1160 495 v -346.173 1159.24 5 v -346.173 1159.24 5 v -350 1160 495 v -350 1160 5 v -350 1160 495 v -353.827 1159.24 495 v -350 1160 5 v -350 1160 5 v -353.827 1159.24 495 v -353.827 1159.24 5 v -353.827 1159.24 495 v -357.071 1157.07 495 v -353.827 1159.24 5 v -353.827 1159.24 5 v -357.071 1157.07 495 v -357.071 1157.07 5 v -357.071 1157.07 495 v -359.239 1153.83 495 v -357.071 1157.07 5 v -357.071 1157.07 5 v -359.239 1153.83 495 v -359.239 1153.83 5 v -359.239 1153.83 495 v -360 1150 495 v -359.239 1153.83 5 v -359.239 1153.83 5 v -360 1150 495 v -360 1150 5 v -360 1450 495 v -359.239 1446.17 495 v -360 1450 5 v -360 1450 5 v -359.239 1446.17 495 v -359.239 1446.17 5 v -359.239 1446.17 495 v -357.071 1442.93 495 v -359.239 1446.17 5 v -359.239 1446.17 5 v -357.071 1442.93 495 v -357.071 1442.93 5 v -357.071 1442.93 495 v -353.827 1440.76 495 v -357.071 1442.93 5 v -357.071 1442.93 5 v -353.827 1440.76 495 v -353.827 1440.76 5 v -353.827 1440.76 495 v -350 1440 495 v -353.827 1440.76 5 v -353.827 1440.76 5 v -350 1440 495 v -350 1440 5 v -350 1440 495 v -346.173 1440.76 495 v -350 1440 5 v -350 1440 5 v -346.173 1440.76 495 v -346.173 1440.76 5 v -346.173 1440.76 495 v -342.929 1442.93 495 v -346.173 1440.76 5 v -346.173 1440.76 5 v -342.929 1442.93 495 v -342.929 1442.93 5 v -342.929 1442.93 495 v -340.761 1446.17 495 v -342.929 1442.93 5 v -342.929 1442.93 5 v -340.761 1446.17 495 v -340.761 1446.17 5 v -340.761 1446.17 495 v -340 1450 495 v -340.761 1446.17 5 v -340.761 1446.17 5 v -340 1450 495 v -340 1450 5 v -340 1450 495 v -340.761 1453.83 495 v -340 1450 5 v -340 1450 5 v -340.761 1453.83 495 v -340.761 1453.83 5 v -340.761 1453.83 495 v -342.929 1457.07 495 v -340.761 1453.83 5 v -340.761 1453.83 5 v -342.929 1457.07 495 v -342.929 1457.07 5 v -342.929 1457.07 495 v -346.173 1459.24 495 v -342.929 1457.07 5 v -342.929 1457.07 5 v -346.173 1459.24 495 v -346.173 1459.24 5 v -346.173 1459.24 495 v -350 1460 495 v -346.173 1459.24 5 v -346.173 1459.24 5 v -350 1460 495 v -350 1460 5 v -350 1460 495 v -353.827 1459.24 495 v -350 1460 5 v -350 1460 5 v -353.827 1459.24 495 v -353.827 1459.24 5 v -353.827 1459.24 495 v -357.071 1457.07 495 v -353.827 1459.24 5 v -353.827 1459.24 5 v -357.071 1457.07 495 v -357.071 1457.07 5 v -357.071 1457.07 495 v -359.239 1453.83 495 v -357.071 1457.07 5 v -357.071 1457.07 5 v -359.239 1453.83 495 v -359.239 1453.83 5 v -359.239 1453.83 495 v -360 1450 495 v -359.239 1453.83 5 v -359.239 1453.83 5 v -360 1450 495 v -360 1450 5 v -660 1450 495 v -659.239 1446.17 495 v -660 1450 5 v -660 1450 5 v -659.239 1446.17 495 v -659.239 1446.17 5 v -659.239 1446.17 495 v -657.071 1442.93 495 v -659.239 1446.17 5 v -659.239 1446.17 5 v -657.071 1442.93 495 v -657.071 1442.93 5 v -657.071 1442.93 495 v -653.827 1440.76 495 v -657.071 1442.93 5 v -657.071 1442.93 5 v -653.827 1440.76 495 v -653.827 1440.76 5 v -653.827 1440.76 495 v -650 1440 495 v -653.827 1440.76 5 v -653.827 1440.76 5 v -650 1440 495 v -650 1440 5 v -650 1440 495 v -646.173 1440.76 495 v -650 1440 5 v -650 1440 5 v -646.173 1440.76 495 v -646.173 1440.76 5 v -646.173 1440.76 495 v -642.929 1442.93 495 v -646.173 1440.76 5 v -646.173 1440.76 5 v -642.929 1442.93 495 v -642.929 1442.93 5 v -642.929 1442.93 495 v -640.761 1446.17 495 v -642.929 1442.93 5 v -642.929 1442.93 5 v -640.761 1446.17 495 v -640.761 1446.17 5 v -640.761 1446.17 495 v -640 1450 495 v -640.761 1446.17 5 v -640.761 1446.17 5 v -640 1450 495 v -640 1450 5 v -640 1450 495 v -640.761 1453.83 495 v -640 1450 5 v -640 1450 5 v -640.761 1453.83 495 v -640.761 1453.83 5 v -640.761 1453.83 495 v -642.929 1457.07 495 v -640.761 1453.83 5 v -640.761 1453.83 5 v -642.929 1457.07 495 v -642.929 1457.07 5 v -642.929 1457.07 495 v -646.173 1459.24 495 v -642.929 1457.07 5 v -642.929 1457.07 5 v -646.173 1459.24 495 v -646.173 1459.24 5 v -646.173 1459.24 495 v -650 1460 495 v -646.173 1459.24 5 v -646.173 1459.24 5 v -650 1460 495 v -650 1460 5 v -650 1460 495 v -653.827 1459.24 495 v -650 1460 5 v -650 1460 5 v -653.827 1459.24 495 v -653.827 1459.24 5 v -653.827 1459.24 495 v -657.071 1457.07 495 v -653.827 1459.24 5 v -653.827 1459.24 5 v -657.071 1457.07 495 v -657.071 1457.07 5 v -657.071 1457.07 495 v -659.239 1453.83 495 v -657.071 1457.07 5 v -657.071 1457.07 5 v -659.239 1453.83 495 v -659.239 1453.83 5 v -659.239 1453.83 495 v -660 1450 495 v -659.239 1453.83 5 v -659.239 1453.83 5 v -660 1450 495 v -660 1450 5 v -300 1500 510 v -700 1500 510 v -300 1100 510 v -300 1100 510 v -700 1500 510 v -700 1100 510 v -700 1500 500 v -300 1500 500 v -700 1100 500 v -700 1100 500 v -300 1500 500 v -300 1100 500 v -700 1500 510 v -700 1500 500 v -700 1100 510 v -700 1100 510 v -700 1500 500 v -700 1100 500 v -300 1500 500 v -300 1500 510 v -300 1100 500 v -300 1100 500 v -300 1500 510 v -300 1100 510 v -300 1100 500 v -300 1100 510 v -700 1100 500 v -700 1100 500 v -300 1100 510 v -700 1100 510 v -300 1500 500 v -700 1500 500 v -300 1500 510 v -300 1500 510 v -700 1500 500 v -700 1500 510 v -560 1095 810 v -559.239 1091.17 810 v -560 1095 510 v -560 1095 510 v -559.239 1091.17 810 v -559.239 1091.17 510 v -559.239 1091.17 810 v -557.071 1087.93 810 v -559.239 1091.17 510 v -559.239 1091.17 510 v -557.071 1087.93 810 v -557.071 1087.93 510 v -557.071 1087.93 810 v -553.827 1085.76 810 v -557.071 1087.93 510 v -557.071 1087.93 510 v -553.827 1085.76 810 v -553.827 1085.76 510 v -553.827 1085.76 810 v -550 1085 810 v -553.827 1085.76 510 v -553.827 1085.76 510 v -550 1085 810 v -550 1085 510 v -550 1085 810 v -546.173 1085.76 810 v -550 1085 510 v -550 1085 510 v -546.173 1085.76 810 v -546.173 1085.76 510 v -546.173 1085.76 810 v -542.929 1087.93 810 v -546.173 1085.76 510 v -546.173 1085.76 510 v -542.929 1087.93 810 v -542.929 1087.93 510 v -542.929 1087.93 810 v -540.761 1091.17 810 v -542.929 1087.93 510 v -542.929 1087.93 510 v -540.761 1091.17 810 v -540.761 1091.17 510 v -540.761 1091.17 810 v -540 1095 810 v -540.761 1091.17 510 v -540.761 1091.17 510 v -540 1095 810 v -540 1095 510 v -540 1095 810 v -540.761 1098.83 810 v -540 1095 510 v -540 1095 510 v -540.761 1098.83 810 v -540.761 1098.83 510 v -540.761 1098.83 810 v -542.929 1102.07 810 v -540.761 1098.83 510 v -540.761 1098.83 510 v -542.929 1102.07 810 v -542.929 1102.07 510 v -542.929 1102.07 810 v -546.173 1104.24 810 v -542.929 1102.07 510 v -542.929 1102.07 510 v -546.173 1104.24 810 v -546.173 1104.24 510 v -546.173 1104.24 810 v -550 1105 810 v -546.173 1104.24 510 v -546.173 1104.24 510 v -550 1105 810 v -550 1105 510 v -550 1105 810 v -553.827 1104.24 810 v -550 1105 510 v -550 1105 510 v -553.827 1104.24 810 v -553.827 1104.24 510 v -553.827 1104.24 810 v -557.071 1102.07 810 v -553.827 1104.24 510 v -553.827 1104.24 510 v -557.071 1102.07 810 v -557.071 1102.07 510 v -557.071 1102.07 810 v -559.239 1098.83 810 v -557.071 1102.07 510 v -557.071 1102.07 510 v -559.239 1098.83 810 v -559.239 1098.83 510 v -559.239 1098.83 810 v -560 1095 810 v -559.239 1098.83 510 v -559.239 1098.83 510 v -560 1095 810 v -560 1095 510 v -460 1095 810 v -459.239 1091.17 810 v -460 1095 510 v -460 1095 510 v -459.239 1091.17 810 v -459.239 1091.17 510 v -459.239 1091.17 810 v -457.071 1087.93 810 v -459.239 1091.17 510 v -459.239 1091.17 510 v -457.071 1087.93 810 v -457.071 1087.93 510 v -457.071 1087.93 810 v -453.827 1085.76 810 v -457.071 1087.93 510 v -457.071 1087.93 510 v -453.827 1085.76 810 v -453.827 1085.76 510 v -453.827 1085.76 810 v -450 1085 810 v -453.827 1085.76 510 v -453.827 1085.76 510 v -450 1085 810 v -450 1085 510 v -450 1085 810 v -446.173 1085.76 810 v -450 1085 510 v -450 1085 510 v -446.173 1085.76 810 v -446.173 1085.76 510 v -446.173 1085.76 810 v -442.929 1087.93 810 v -446.173 1085.76 510 v -446.173 1085.76 510 v -442.929 1087.93 810 v -442.929 1087.93 510 v -442.929 1087.93 810 v -440.761 1091.17 810 v -442.929 1087.93 510 v -442.929 1087.93 510 v -440.761 1091.17 810 v -440.761 1091.17 510 v -440.761 1091.17 810 v -440 1095 810 v -440.761 1091.17 510 v -440.761 1091.17 510 v -440 1095 810 v -440 1095 510 v -440 1095 810 v -440.761 1098.83 810 v -440 1095 510 v -440 1095 510 v -440.761 1098.83 810 v -440.761 1098.83 510 v -440.761 1098.83 810 v -442.929 1102.07 810 v -440.761 1098.83 510 v -440.761 1098.83 510 v -442.929 1102.07 810 v -442.929 1102.07 510 v -442.929 1102.07 810 v -446.173 1104.24 810 v -442.929 1102.07 510 v -442.929 1102.07 510 v -446.173 1104.24 810 v -446.173 1104.24 510 v -446.173 1104.24 810 v -450 1105 810 v -446.173 1104.24 510 v -446.173 1104.24 510 v -450 1105 810 v -450 1105 510 v -450 1105 810 v -453.827 1104.24 810 v -450 1105 510 v -450 1105 510 v -453.827 1104.24 810 v -453.827 1104.24 510 v -453.827 1104.24 810 v -457.071 1102.07 810 v -453.827 1104.24 510 v -453.827 1104.24 510 v -457.071 1102.07 810 v -457.071 1102.07 510 v -457.071 1102.07 810 v -459.239 1098.83 810 v -457.071 1102.07 510 v -457.071 1102.07 510 v -459.239 1098.83 810 v -459.239 1098.83 510 v -459.239 1098.83 810 v -460 1095 810 v -459.239 1098.83 510 v -459.239 1098.83 510 v -460 1095 810 v -460 1095 510 v -300 1110 910 v -700 1110 910 v -300 1100 910 v -300 1100 910 v -700 1110 910 v -700 1100 910 v -700 1110 710 v -300 1110 710 v -700 1100 710 v -700 1100 710 v -300 1110 710 v -300 1100 710 v -700 1110 910 v -700 1110 710 v -700 1100 910 v -700 1100 910 v -700 1110 710 v -700 1100 710 v -300 1110 710 v -300 1110 910 v -300 1100 710 v -300 1100 710 v -300 1110 910 v -300 1100 910 v -300 1100 710 v -300 1100 910 v -700 1100 710 v -700 1100 710 v -300 1100 910 v -700 1100 910 v -300 1110 710 v -700 1110 710 v -300 1110 910 v -300 1110 910 v -700 1110 710 v -700 1110 910 v -1660 1150 495 v -1659.24 1146.17 495 v -1660 1150 5 v -1660 1150 5 v -1659.24 1146.17 495 v -1659.24 1146.17 5 v -1659.24 1146.17 495 v -1657.07 1142.93 495 v -1659.24 1146.17 5 v -1659.24 1146.17 5 v -1657.07 1142.93 495 v -1657.07 1142.93 5 v -1657.07 1142.93 495 v -1653.83 1140.76 495 v -1657.07 1142.93 5 v -1657.07 1142.93 5 v -1653.83 1140.76 495 v -1653.83 1140.76 5 v -1653.83 1140.76 495 v -1650 1140 495 v -1653.83 1140.76 5 v -1653.83 1140.76 5 v -1650 1140 495 v -1650 1140 5 v -1650 1140 495 v -1646.17 1140.76 495 v -1650 1140 5 v -1650 1140 5 v -1646.17 1140.76 495 v -1646.17 1140.76 5 v -1646.17 1140.76 495 v -1642.93 1142.93 495 v -1646.17 1140.76 5 v -1646.17 1140.76 5 v -1642.93 1142.93 495 v -1642.93 1142.93 5 v -1642.93 1142.93 495 v -1640.76 1146.17 495 v -1642.93 1142.93 5 v -1642.93 1142.93 5 v -1640.76 1146.17 495 v -1640.76 1146.17 5 v -1640.76 1146.17 495 v -1640 1150 495 v -1640.76 1146.17 5 v -1640.76 1146.17 5 v -1640 1150 495 v -1640 1150 5 v -1640 1150 495 v -1640.76 1153.83 495 v -1640 1150 5 v -1640 1150 5 v -1640.76 1153.83 495 v -1640.76 1153.83 5 v -1640.76 1153.83 495 v -1642.93 1157.07 495 v -1640.76 1153.83 5 v -1640.76 1153.83 5 v -1642.93 1157.07 495 v -1642.93 1157.07 5 v -1642.93 1157.07 495 v -1646.17 1159.24 495 v -1642.93 1157.07 5 v -1642.93 1157.07 5 v -1646.17 1159.24 495 v -1646.17 1159.24 5 v -1646.17 1159.24 495 v -1650 1160 495 v -1646.17 1159.24 5 v -1646.17 1159.24 5 v -1650 1160 495 v -1650 1160 5 v -1650 1160 495 v -1653.83 1159.24 495 v -1650 1160 5 v -1650 1160 5 v -1653.83 1159.24 495 v -1653.83 1159.24 5 v -1653.83 1159.24 495 v -1657.07 1157.07 495 v -1653.83 1159.24 5 v -1653.83 1159.24 5 v -1657.07 1157.07 495 v -1657.07 1157.07 5 v -1657.07 1157.07 495 v -1659.24 1153.83 495 v -1657.07 1157.07 5 v -1657.07 1157.07 5 v -1659.24 1153.83 495 v -1659.24 1153.83 5 v -1659.24 1153.83 495 v -1660 1150 495 v -1659.24 1153.83 5 v -1659.24 1153.83 5 v -1660 1150 495 v -1660 1150 5 v -1360 1150 495 v -1359.24 1146.17 495 v -1360 1150 5 v -1360 1150 5 v -1359.24 1146.17 495 v -1359.24 1146.17 5 v -1359.24 1146.17 495 v -1357.07 1142.93 495 v -1359.24 1146.17 5 v -1359.24 1146.17 5 v -1357.07 1142.93 495 v -1357.07 1142.93 5 v -1357.07 1142.93 495 v -1353.83 1140.76 495 v -1357.07 1142.93 5 v -1357.07 1142.93 5 v -1353.83 1140.76 495 v -1353.83 1140.76 5 v -1353.83 1140.76 495 v -1350 1140 495 v -1353.83 1140.76 5 v -1353.83 1140.76 5 v -1350 1140 495 v -1350 1140 5 v -1350 1140 495 v -1346.17 1140.76 495 v -1350 1140 5 v -1350 1140 5 v -1346.17 1140.76 495 v -1346.17 1140.76 5 v -1346.17 1140.76 495 v -1342.93 1142.93 495 v -1346.17 1140.76 5 v -1346.17 1140.76 5 v -1342.93 1142.93 495 v -1342.93 1142.93 5 v -1342.93 1142.93 495 v -1340.76 1146.17 495 v -1342.93 1142.93 5 v -1342.93 1142.93 5 v -1340.76 1146.17 495 v -1340.76 1146.17 5 v -1340.76 1146.17 495 v -1340 1150 495 v -1340.76 1146.17 5 v -1340.76 1146.17 5 v -1340 1150 495 v -1340 1150 5 v -1340 1150 495 v -1340.76 1153.83 495 v -1340 1150 5 v -1340 1150 5 v -1340.76 1153.83 495 v -1340.76 1153.83 5 v -1340.76 1153.83 495 v -1342.93 1157.07 495 v -1340.76 1153.83 5 v -1340.76 1153.83 5 v -1342.93 1157.07 495 v -1342.93 1157.07 5 v -1342.93 1157.07 495 v -1346.17 1159.24 495 v -1342.93 1157.07 5 v -1342.93 1157.07 5 v -1346.17 1159.24 495 v -1346.17 1159.24 5 v -1346.17 1159.24 495 v -1350 1160 495 v -1346.17 1159.24 5 v -1346.17 1159.24 5 v -1350 1160 495 v -1350 1160 5 v -1350 1160 495 v -1353.83 1159.24 495 v -1350 1160 5 v -1350 1160 5 v -1353.83 1159.24 495 v -1353.83 1159.24 5 v -1353.83 1159.24 495 v -1357.07 1157.07 495 v -1353.83 1159.24 5 v -1353.83 1159.24 5 v -1357.07 1157.07 495 v -1357.07 1157.07 5 v -1357.07 1157.07 495 v -1359.24 1153.83 495 v -1357.07 1157.07 5 v -1357.07 1157.07 5 v -1359.24 1153.83 495 v -1359.24 1153.83 5 v -1359.24 1153.83 495 v -1360 1150 495 v -1359.24 1153.83 5 v -1359.24 1153.83 5 v -1360 1150 495 v -1360 1150 5 v -1360 1450 495 v -1359.24 1446.17 495 v -1360 1450 5 v -1360 1450 5 v -1359.24 1446.17 495 v -1359.24 1446.17 5 v -1359.24 1446.17 495 v -1357.07 1442.93 495 v -1359.24 1446.17 5 v -1359.24 1446.17 5 v -1357.07 1442.93 495 v -1357.07 1442.93 5 v -1357.07 1442.93 495 v -1353.83 1440.76 495 v -1357.07 1442.93 5 v -1357.07 1442.93 5 v -1353.83 1440.76 495 v -1353.83 1440.76 5 v -1353.83 1440.76 495 v -1350 1440 495 v -1353.83 1440.76 5 v -1353.83 1440.76 5 v -1350 1440 495 v -1350 1440 5 v -1350 1440 495 v -1346.17 1440.76 495 v -1350 1440 5 v -1350 1440 5 v -1346.17 1440.76 495 v -1346.17 1440.76 5 v -1346.17 1440.76 495 v -1342.93 1442.93 495 v -1346.17 1440.76 5 v -1346.17 1440.76 5 v -1342.93 1442.93 495 v -1342.93 1442.93 5 v -1342.93 1442.93 495 v -1340.76 1446.17 495 v -1342.93 1442.93 5 v -1342.93 1442.93 5 v -1340.76 1446.17 495 v -1340.76 1446.17 5 v -1340.76 1446.17 495 v -1340 1450 495 v -1340.76 1446.17 5 v -1340.76 1446.17 5 v -1340 1450 495 v -1340 1450 5 v -1340 1450 495 v -1340.76 1453.83 495 v -1340 1450 5 v -1340 1450 5 v -1340.76 1453.83 495 v -1340.76 1453.83 5 v -1340.76 1453.83 495 v -1342.93 1457.07 495 v -1340.76 1453.83 5 v -1340.76 1453.83 5 v -1342.93 1457.07 495 v -1342.93 1457.07 5 v -1342.93 1457.07 495 v -1346.17 1459.24 495 v -1342.93 1457.07 5 v -1342.93 1457.07 5 v -1346.17 1459.24 495 v -1346.17 1459.24 5 v -1346.17 1459.24 495 v -1350 1460 495 v -1346.17 1459.24 5 v -1346.17 1459.24 5 v -1350 1460 495 v -1350 1460 5 v -1350 1460 495 v -1353.83 1459.24 495 v -1350 1460 5 v -1350 1460 5 v -1353.83 1459.24 495 v -1353.83 1459.24 5 v -1353.83 1459.24 495 v -1357.07 1457.07 495 v -1353.83 1459.24 5 v -1353.83 1459.24 5 v -1357.07 1457.07 495 v -1357.07 1457.07 5 v -1357.07 1457.07 495 v -1359.24 1453.83 495 v -1357.07 1457.07 5 v -1357.07 1457.07 5 v -1359.24 1453.83 495 v -1359.24 1453.83 5 v -1359.24 1453.83 495 v -1360 1450 495 v -1359.24 1453.83 5 v -1359.24 1453.83 5 v -1360 1450 495 v -1360 1450 5 v -1660 1450 495 v -1659.24 1446.17 495 v -1660 1450 5 v -1660 1450 5 v -1659.24 1446.17 495 v -1659.24 1446.17 5 v -1659.24 1446.17 495 v -1657.07 1442.93 495 v -1659.24 1446.17 5 v -1659.24 1446.17 5 v -1657.07 1442.93 495 v -1657.07 1442.93 5 v -1657.07 1442.93 495 v -1653.83 1440.76 495 v -1657.07 1442.93 5 v -1657.07 1442.93 5 v -1653.83 1440.76 495 v -1653.83 1440.76 5 v -1653.83 1440.76 495 v -1650 1440 495 v -1653.83 1440.76 5 v -1653.83 1440.76 5 v -1650 1440 495 v -1650 1440 5 v -1650 1440 495 v -1646.17 1440.76 495 v -1650 1440 5 v -1650 1440 5 v -1646.17 1440.76 495 v -1646.17 1440.76 5 v -1646.17 1440.76 495 v -1642.93 1442.93 495 v -1646.17 1440.76 5 v -1646.17 1440.76 5 v -1642.93 1442.93 495 v -1642.93 1442.93 5 v -1642.93 1442.93 495 v -1640.76 1446.17 495 v -1642.93 1442.93 5 v -1642.93 1442.93 5 v -1640.76 1446.17 495 v -1640.76 1446.17 5 v -1640.76 1446.17 495 v -1640 1450 495 v -1640.76 1446.17 5 v -1640.76 1446.17 5 v -1640 1450 495 v -1640 1450 5 v -1640 1450 495 v -1640.76 1453.83 495 v -1640 1450 5 v -1640 1450 5 v -1640.76 1453.83 495 v -1640.76 1453.83 5 v -1640.76 1453.83 495 v -1642.93 1457.07 495 v -1640.76 1453.83 5 v -1640.76 1453.83 5 v -1642.93 1457.07 495 v -1642.93 1457.07 5 v -1642.93 1457.07 495 v -1646.17 1459.24 495 v -1642.93 1457.07 5 v -1642.93 1457.07 5 v -1646.17 1459.24 495 v -1646.17 1459.24 5 v -1646.17 1459.24 495 v -1650 1460 495 v -1646.17 1459.24 5 v -1646.17 1459.24 5 v -1650 1460 495 v -1650 1460 5 v -1650 1460 495 v -1653.83 1459.24 495 v -1650 1460 5 v -1650 1460 5 v -1653.83 1459.24 495 v -1653.83 1459.24 5 v -1653.83 1459.24 495 v -1657.07 1457.07 495 v -1653.83 1459.24 5 v -1653.83 1459.24 5 v -1657.07 1457.07 495 v -1657.07 1457.07 5 v -1657.07 1457.07 495 v -1659.24 1453.83 495 v -1657.07 1457.07 5 v -1657.07 1457.07 5 v -1659.24 1453.83 495 v -1659.24 1453.83 5 v -1659.24 1453.83 495 v -1660 1450 495 v -1659.24 1453.83 5 v -1659.24 1453.83 5 v -1660 1450 495 v -1660 1450 5 v -1300 1500 510 v -1700 1500 510 v -1300 1100 510 v -1300 1100 510 v -1700 1500 510 v -1700 1100 510 v -1700 1500 500 v -1300 1500 500 v -1700 1100 500 v -1700 1100 500 v -1300 1500 500 v -1300 1100 500 v -1700 1500 510 v -1700 1500 500 v -1700 1100 510 v -1700 1100 510 v -1700 1500 500 v -1700 1100 500 v -1300 1500 500 v -1300 1500 510 v -1300 1100 500 v -1300 1100 500 v -1300 1500 510 v -1300 1100 510 v -1300 1100 500 v -1300 1100 510 v -1700 1100 500 v -1700 1100 500 v -1300 1100 510 v -1700 1100 510 v -1300 1500 500 v -1700 1500 500 v -1300 1500 510 v -1300 1500 510 v -1700 1500 500 v -1700 1500 510 v -1560 1095 810 v -1559.24 1091.17 810 v -1560 1095 510 v -1560 1095 510 v -1559.24 1091.17 810 v -1559.24 1091.17 510 v -1559.24 1091.17 810 v -1557.07 1087.93 810 v -1559.24 1091.17 510 v -1559.24 1091.17 510 v -1557.07 1087.93 810 v -1557.07 1087.93 510 v -1557.07 1087.93 810 v -1553.83 1085.76 810 v -1557.07 1087.93 510 v -1557.07 1087.93 510 v -1553.83 1085.76 810 v -1553.83 1085.76 510 v -1553.83 1085.76 810 v -1550 1085 810 v -1553.83 1085.76 510 v -1553.83 1085.76 510 v -1550 1085 810 v -1550 1085 510 v -1550 1085 810 v -1546.17 1085.76 810 v -1550 1085 510 v -1550 1085 510 v -1546.17 1085.76 810 v -1546.17 1085.76 510 v -1546.17 1085.76 810 v -1542.93 1087.93 810 v -1546.17 1085.76 510 v -1546.17 1085.76 510 v -1542.93 1087.93 810 v -1542.93 1087.93 510 v -1542.93 1087.93 810 v -1540.76 1091.17 810 v -1542.93 1087.93 510 v -1542.93 1087.93 510 v -1540.76 1091.17 810 v -1540.76 1091.17 510 v -1540.76 1091.17 810 v -1540 1095 810 v -1540.76 1091.17 510 v -1540.76 1091.17 510 v -1540 1095 810 v -1540 1095 510 v -1540 1095 810 v -1540.76 1098.83 810 v -1540 1095 510 v -1540 1095 510 v -1540.76 1098.83 810 v -1540.76 1098.83 510 v -1540.76 1098.83 810 v -1542.93 1102.07 810 v -1540.76 1098.83 510 v -1540.76 1098.83 510 v -1542.93 1102.07 810 v -1542.93 1102.07 510 v -1542.93 1102.07 810 v -1546.17 1104.24 810 v -1542.93 1102.07 510 v -1542.93 1102.07 510 v -1546.17 1104.24 810 v -1546.17 1104.24 510 v -1546.17 1104.24 810 v -1550 1105 810 v -1546.17 1104.24 510 v -1546.17 1104.24 510 v -1550 1105 810 v -1550 1105 510 v -1550 1105 810 v -1553.83 1104.24 810 v -1550 1105 510 v -1550 1105 510 v -1553.83 1104.24 810 v -1553.83 1104.24 510 v -1553.83 1104.24 810 v -1557.07 1102.07 810 v -1553.83 1104.24 510 v -1553.83 1104.24 510 v -1557.07 1102.07 810 v -1557.07 1102.07 510 v -1557.07 1102.07 810 v -1559.24 1098.83 810 v -1557.07 1102.07 510 v -1557.07 1102.07 510 v -1559.24 1098.83 810 v -1559.24 1098.83 510 v -1559.24 1098.83 810 v -1560 1095 810 v -1559.24 1098.83 510 v -1559.24 1098.83 510 v -1560 1095 810 v -1560 1095 510 v -1460 1095 810 v -1459.24 1091.17 810 v -1460 1095 510 v -1460 1095 510 v -1459.24 1091.17 810 v -1459.24 1091.17 510 v -1459.24 1091.17 810 v -1457.07 1087.93 810 v -1459.24 1091.17 510 v -1459.24 1091.17 510 v -1457.07 1087.93 810 v -1457.07 1087.93 510 v -1457.07 1087.93 810 v -1453.83 1085.76 810 v -1457.07 1087.93 510 v -1457.07 1087.93 510 v -1453.83 1085.76 810 v -1453.83 1085.76 510 v -1453.83 1085.76 810 v -1450 1085 810 v -1453.83 1085.76 510 v -1453.83 1085.76 510 v -1450 1085 810 v -1450 1085 510 v -1450 1085 810 v -1446.17 1085.76 810 v -1450 1085 510 v -1450 1085 510 v -1446.17 1085.76 810 v -1446.17 1085.76 510 v -1446.17 1085.76 810 v -1442.93 1087.93 810 v -1446.17 1085.76 510 v -1446.17 1085.76 510 v -1442.93 1087.93 810 v -1442.93 1087.93 510 v -1442.93 1087.93 810 v -1440.76 1091.17 810 v -1442.93 1087.93 510 v -1442.93 1087.93 510 v -1440.76 1091.17 810 v -1440.76 1091.17 510 v -1440.76 1091.17 810 v -1440 1095 810 v -1440.76 1091.17 510 v -1440.76 1091.17 510 v -1440 1095 810 v -1440 1095 510 v -1440 1095 810 v -1440.76 1098.83 810 v -1440 1095 510 v -1440 1095 510 v -1440.76 1098.83 810 v -1440.76 1098.83 510 v -1440.76 1098.83 810 v -1442.93 1102.07 810 v -1440.76 1098.83 510 v -1440.76 1098.83 510 v -1442.93 1102.07 810 v -1442.93 1102.07 510 v -1442.93 1102.07 810 v -1446.17 1104.24 810 v -1442.93 1102.07 510 v -1442.93 1102.07 510 v -1446.17 1104.24 810 v -1446.17 1104.24 510 v -1446.17 1104.24 810 v -1450 1105 810 v -1446.17 1104.24 510 v -1446.17 1104.24 510 v -1450 1105 810 v -1450 1105 510 v -1450 1105 810 v -1453.83 1104.24 810 v -1450 1105 510 v -1450 1105 510 v -1453.83 1104.24 810 v -1453.83 1104.24 510 v -1453.83 1104.24 810 v -1457.07 1102.07 810 v -1453.83 1104.24 510 v -1453.83 1104.24 510 v -1457.07 1102.07 810 v -1457.07 1102.07 510 v -1457.07 1102.07 810 v -1459.24 1098.83 810 v -1457.07 1102.07 510 v -1457.07 1102.07 510 v -1459.24 1098.83 810 v -1459.24 1098.83 510 v -1459.24 1098.83 810 v -1460 1095 810 v -1459.24 1098.83 510 v -1459.24 1098.83 510 v -1460 1095 810 v -1460 1095 510 v -1300 1110 910 v -1700 1110 910 v -1300 1100 910 v -1300 1100 910 v -1700 1110 910 v -1700 1100 910 v -1700 1110 710 v -1300 1110 710 v -1700 1100 710 v -1700 1100 710 v -1300 1110 710 v -1300 1100 710 v -1700 1110 910 v -1700 1110 710 v -1700 1100 910 v -1700 1100 910 v -1700 1110 710 v -1700 1100 710 v -1300 1110 710 v -1300 1110 910 v -1300 1100 710 v -1300 1100 710 v -1300 1110 910 v -1300 1100 910 v -1300 1100 710 v -1300 1100 910 v -1700 1100 710 v -1700 1100 710 v -1300 1100 910 v -1700 1100 910 v -1300 1110 710 v -1700 1110 710 v -1300 1110 910 v -1300 1110 910 v -1700 1110 710 v -1700 1110 910 v -2660 1150 495 v -2659.24 1146.17 495 v -2660 1150 5 v -2660 1150 5 v -2659.24 1146.17 495 v -2659.24 1146.17 5 v -2659.24 1146.17 495 v -2657.07 1142.93 495 v -2659.24 1146.17 5 v -2659.24 1146.17 5 v -2657.07 1142.93 495 v -2657.07 1142.93 5 v -2657.07 1142.93 495 v -2653.83 1140.76 495 v -2657.07 1142.93 5 v -2657.07 1142.93 5 v -2653.83 1140.76 495 v -2653.83 1140.76 5 v -2653.83 1140.76 495 v -2650 1140 495 v -2653.83 1140.76 5 v -2653.83 1140.76 5 v -2650 1140 495 v -2650 1140 5 v -2650 1140 495 v -2646.17 1140.76 495 v -2650 1140 5 v -2650 1140 5 v -2646.17 1140.76 495 v -2646.17 1140.76 5 v -2646.17 1140.76 495 v -2642.93 1142.93 495 v -2646.17 1140.76 5 v -2646.17 1140.76 5 v -2642.93 1142.93 495 v -2642.93 1142.93 5 v -2642.93 1142.93 495 v -2640.76 1146.17 495 v -2642.93 1142.93 5 v -2642.93 1142.93 5 v -2640.76 1146.17 495 v -2640.76 1146.17 5 v -2640.76 1146.17 495 v -2640 1150 495 v -2640.76 1146.17 5 v -2640.76 1146.17 5 v -2640 1150 495 v -2640 1150 5 v -2640 1150 495 v -2640.76 1153.83 495 v -2640 1150 5 v -2640 1150 5 v -2640.76 1153.83 495 v -2640.76 1153.83 5 v -2640.76 1153.83 495 v -2642.93 1157.07 495 v -2640.76 1153.83 5 v -2640.76 1153.83 5 v -2642.93 1157.07 495 v -2642.93 1157.07 5 v -2642.93 1157.07 495 v -2646.17 1159.24 495 v -2642.93 1157.07 5 v -2642.93 1157.07 5 v -2646.17 1159.24 495 v -2646.17 1159.24 5 v -2646.17 1159.24 495 v -2650 1160 495 v -2646.17 1159.24 5 v -2646.17 1159.24 5 v -2650 1160 495 v -2650 1160 5 v -2650 1160 495 v -2653.83 1159.24 495 v -2650 1160 5 v -2650 1160 5 v -2653.83 1159.24 495 v -2653.83 1159.24 5 v -2653.83 1159.24 495 v -2657.07 1157.07 495 v -2653.83 1159.24 5 v -2653.83 1159.24 5 v -2657.07 1157.07 495 v -2657.07 1157.07 5 v -2657.07 1157.07 495 v -2659.24 1153.83 495 v -2657.07 1157.07 5 v -2657.07 1157.07 5 v -2659.24 1153.83 495 v -2659.24 1153.83 5 v -2659.24 1153.83 495 v -2660 1150 495 v -2659.24 1153.83 5 v -2659.24 1153.83 5 v -2660 1150 495 v -2660 1150 5 v -2360 1150 495 v -2359.24 1146.17 495 v -2360 1150 5 v -2360 1150 5 v -2359.24 1146.17 495 v -2359.24 1146.17 5 v -2359.24 1146.17 495 v -2357.07 1142.93 495 v -2359.24 1146.17 5 v -2359.24 1146.17 5 v -2357.07 1142.93 495 v -2357.07 1142.93 5 v -2357.07 1142.93 495 v -2353.83 1140.76 495 v -2357.07 1142.93 5 v -2357.07 1142.93 5 v -2353.83 1140.76 495 v -2353.83 1140.76 5 v -2353.83 1140.76 495 v -2350 1140 495 v -2353.83 1140.76 5 v -2353.83 1140.76 5 v -2350 1140 495 v -2350 1140 5 v -2350 1140 495 v -2346.17 1140.76 495 v -2350 1140 5 v -2350 1140 5 v -2346.17 1140.76 495 v -2346.17 1140.76 5 v -2346.17 1140.76 495 v -2342.93 1142.93 495 v -2346.17 1140.76 5 v -2346.17 1140.76 5 v -2342.93 1142.93 495 v -2342.93 1142.93 5 v -2342.93 1142.93 495 v -2340.76 1146.17 495 v -2342.93 1142.93 5 v -2342.93 1142.93 5 v -2340.76 1146.17 495 v -2340.76 1146.17 5 v -2340.76 1146.17 495 v -2340 1150 495 v -2340.76 1146.17 5 v -2340.76 1146.17 5 v -2340 1150 495 v -2340 1150 5 v -2340 1150 495 v -2340.76 1153.83 495 v -2340 1150 5 v -2340 1150 5 v -2340.76 1153.83 495 v -2340.76 1153.83 5 v -2340.76 1153.83 495 v -2342.93 1157.07 495 v -2340.76 1153.83 5 v -2340.76 1153.83 5 v -2342.93 1157.07 495 v -2342.93 1157.07 5 v -2342.93 1157.07 495 v -2346.17 1159.24 495 v -2342.93 1157.07 5 v -2342.93 1157.07 5 v -2346.17 1159.24 495 v -2346.17 1159.24 5 v -2346.17 1159.24 495 v -2350 1160 495 v -2346.17 1159.24 5 v -2346.17 1159.24 5 v -2350 1160 495 v -2350 1160 5 v -2350 1160 495 v -2353.83 1159.24 495 v -2350 1160 5 v -2350 1160 5 v -2353.83 1159.24 495 v -2353.83 1159.24 5 v -2353.83 1159.24 495 v -2357.07 1157.07 495 v -2353.83 1159.24 5 v -2353.83 1159.24 5 v -2357.07 1157.07 495 v -2357.07 1157.07 5 v -2357.07 1157.07 495 v -2359.24 1153.83 495 v -2357.07 1157.07 5 v -2357.07 1157.07 5 v -2359.24 1153.83 495 v -2359.24 1153.83 5 v -2359.24 1153.83 495 v -2360 1150 495 v -2359.24 1153.83 5 v -2359.24 1153.83 5 v -2360 1150 495 v -2360 1150 5 v -2360 1450 495 v -2359.24 1446.17 495 v -2360 1450 5 v -2360 1450 5 v -2359.24 1446.17 495 v -2359.24 1446.17 5 v -2359.24 1446.17 495 v -2357.07 1442.93 495 v -2359.24 1446.17 5 v -2359.24 1446.17 5 v -2357.07 1442.93 495 v -2357.07 1442.93 5 v -2357.07 1442.93 495 v -2353.83 1440.76 495 v -2357.07 1442.93 5 v -2357.07 1442.93 5 v -2353.83 1440.76 495 v -2353.83 1440.76 5 v -2353.83 1440.76 495 v -2350 1440 495 v -2353.83 1440.76 5 v -2353.83 1440.76 5 v -2350 1440 495 v -2350 1440 5 v -2350 1440 495 v -2346.17 1440.76 495 v -2350 1440 5 v -2350 1440 5 v -2346.17 1440.76 495 v -2346.17 1440.76 5 v -2346.17 1440.76 495 v -2342.93 1442.93 495 v -2346.17 1440.76 5 v -2346.17 1440.76 5 v -2342.93 1442.93 495 v -2342.93 1442.93 5 v -2342.93 1442.93 495 v -2340.76 1446.17 495 v -2342.93 1442.93 5 v -2342.93 1442.93 5 v -2340.76 1446.17 495 v -2340.76 1446.17 5 v -2340.76 1446.17 495 v -2340 1450 495 v -2340.76 1446.17 5 v -2340.76 1446.17 5 v -2340 1450 495 v -2340 1450 5 v -2340 1450 495 v -2340.76 1453.83 495 v -2340 1450 5 v -2340 1450 5 v -2340.76 1453.83 495 v -2340.76 1453.83 5 v -2340.76 1453.83 495 v -2342.93 1457.07 495 v -2340.76 1453.83 5 v -2340.76 1453.83 5 v -2342.93 1457.07 495 v -2342.93 1457.07 5 v -2342.93 1457.07 495 v -2346.17 1459.24 495 v -2342.93 1457.07 5 v -2342.93 1457.07 5 v -2346.17 1459.24 495 v -2346.17 1459.24 5 v -2346.17 1459.24 495 v -2350 1460 495 v -2346.17 1459.24 5 v -2346.17 1459.24 5 v -2350 1460 495 v -2350 1460 5 v -2350 1460 495 v -2353.83 1459.24 495 v -2350 1460 5 v -2350 1460 5 v -2353.83 1459.24 495 v -2353.83 1459.24 5 v -2353.83 1459.24 495 v -2357.07 1457.07 495 v -2353.83 1459.24 5 v -2353.83 1459.24 5 v -2357.07 1457.07 495 v -2357.07 1457.07 5 v -2357.07 1457.07 495 v -2359.24 1453.83 495 v -2357.07 1457.07 5 v -2357.07 1457.07 5 v -2359.24 1453.83 495 v -2359.24 1453.83 5 v -2359.24 1453.83 495 v -2360 1450 495 v -2359.24 1453.83 5 v -2359.24 1453.83 5 v -2360 1450 495 v -2360 1450 5 v -2660 1450 495 v -2659.24 1446.17 495 v -2660 1450 5 v -2660 1450 5 v -2659.24 1446.17 495 v -2659.24 1446.17 5 v -2659.24 1446.17 495 v -2657.07 1442.93 495 v -2659.24 1446.17 5 v -2659.24 1446.17 5 v -2657.07 1442.93 495 v -2657.07 1442.93 5 v -2657.07 1442.93 495 v -2653.83 1440.76 495 v -2657.07 1442.93 5 v -2657.07 1442.93 5 v -2653.83 1440.76 495 v -2653.83 1440.76 5 v -2653.83 1440.76 495 v -2650 1440 495 v -2653.83 1440.76 5 v -2653.83 1440.76 5 v -2650 1440 495 v -2650 1440 5 v -2650 1440 495 v -2646.17 1440.76 495 v -2650 1440 5 v -2650 1440 5 v -2646.17 1440.76 495 v -2646.17 1440.76 5 v -2646.17 1440.76 495 v -2642.93 1442.93 495 v -2646.17 1440.76 5 v -2646.17 1440.76 5 v -2642.93 1442.93 495 v -2642.93 1442.93 5 v -2642.93 1442.93 495 v -2640.76 1446.17 495 v -2642.93 1442.93 5 v -2642.93 1442.93 5 v -2640.76 1446.17 495 v -2640.76 1446.17 5 v -2640.76 1446.17 495 v -2640 1450 495 v -2640.76 1446.17 5 v -2640.76 1446.17 5 v -2640 1450 495 v -2640 1450 5 v -2640 1450 495 v -2640.76 1453.83 495 v -2640 1450 5 v -2640 1450 5 v -2640.76 1453.83 495 v -2640.76 1453.83 5 v -2640.76 1453.83 495 v -2642.93 1457.07 495 v -2640.76 1453.83 5 v -2640.76 1453.83 5 v -2642.93 1457.07 495 v -2642.93 1457.07 5 v -2642.93 1457.07 495 v -2646.17 1459.24 495 v -2642.93 1457.07 5 v -2642.93 1457.07 5 v -2646.17 1459.24 495 v -2646.17 1459.24 5 v -2646.17 1459.24 495 v -2650 1460 495 v -2646.17 1459.24 5 v -2646.17 1459.24 5 v -2650 1460 495 v -2650 1460 5 v -2650 1460 495 v -2653.83 1459.24 495 v -2650 1460 5 v -2650 1460 5 v -2653.83 1459.24 495 v -2653.83 1459.24 5 v -2653.83 1459.24 495 v -2657.07 1457.07 495 v -2653.83 1459.24 5 v -2653.83 1459.24 5 v -2657.07 1457.07 495 v -2657.07 1457.07 5 v -2657.07 1457.07 495 v -2659.24 1453.83 495 v -2657.07 1457.07 5 v -2657.07 1457.07 5 v -2659.24 1453.83 495 v -2659.24 1453.83 5 v -2659.24 1453.83 495 v -2660 1450 495 v -2659.24 1453.83 5 v -2659.24 1453.83 5 v -2660 1450 495 v -2660 1450 5 v -2300 1500 510 v -2700 1500 510 v -2300 1100 510 v -2300 1100 510 v -2700 1500 510 v -2700 1100 510 v -2700 1500 500 v -2300 1500 500 v -2700 1100 500 v -2700 1100 500 v -2300 1500 500 v -2300 1100 500 v -2700 1500 510 v -2700 1500 500 v -2700 1100 510 v -2700 1100 510 v -2700 1500 500 v -2700 1100 500 v -2300 1500 500 v -2300 1500 510 v -2300 1100 500 v -2300 1100 500 v -2300 1500 510 v -2300 1100 510 v -2300 1100 500 v -2300 1100 510 v -2700 1100 500 v -2700 1100 500 v -2300 1100 510 v -2700 1100 510 v -2300 1500 500 v -2700 1500 500 v -2300 1500 510 v -2300 1500 510 v -2700 1500 500 v -2700 1500 510 v -2560 1095 810 v -2559.24 1091.17 810 v -2560 1095 510 v -2560 1095 510 v -2559.24 1091.17 810 v -2559.24 1091.17 510 v -2559.24 1091.17 810 v -2557.07 1087.93 810 v -2559.24 1091.17 510 v -2559.24 1091.17 510 v -2557.07 1087.93 810 v -2557.07 1087.93 510 v -2557.07 1087.93 810 v -2553.83 1085.76 810 v -2557.07 1087.93 510 v -2557.07 1087.93 510 v -2553.83 1085.76 810 v -2553.83 1085.76 510 v -2553.83 1085.76 810 v -2550 1085 810 v -2553.83 1085.76 510 v -2553.83 1085.76 510 v -2550 1085 810 v -2550 1085 510 v -2550 1085 810 v -2546.17 1085.76 810 v -2550 1085 510 v -2550 1085 510 v -2546.17 1085.76 810 v -2546.17 1085.76 510 v -2546.17 1085.76 810 v -2542.93 1087.93 810 v -2546.17 1085.76 510 v -2546.17 1085.76 510 v -2542.93 1087.93 810 v -2542.93 1087.93 510 v -2542.93 1087.93 810 v -2540.76 1091.17 810 v -2542.93 1087.93 510 v -2542.93 1087.93 510 v -2540.76 1091.17 810 v -2540.76 1091.17 510 v -2540.76 1091.17 810 v -2540 1095 810 v -2540.76 1091.17 510 v -2540.76 1091.17 510 v -2540 1095 810 v -2540 1095 510 v -2540 1095 810 v -2540.76 1098.83 810 v -2540 1095 510 v -2540 1095 510 v -2540.76 1098.83 810 v -2540.76 1098.83 510 v -2540.76 1098.83 810 v -2542.93 1102.07 810 v -2540.76 1098.83 510 v -2540.76 1098.83 510 v -2542.93 1102.07 810 v -2542.93 1102.07 510 v -2542.93 1102.07 810 v -2546.17 1104.24 810 v -2542.93 1102.07 510 v -2542.93 1102.07 510 v -2546.17 1104.24 810 v -2546.17 1104.24 510 v -2546.17 1104.24 810 v -2550 1105 810 v -2546.17 1104.24 510 v -2546.17 1104.24 510 v -2550 1105 810 v -2550 1105 510 v -2550 1105 810 v -2553.83 1104.24 810 v -2550 1105 510 v -2550 1105 510 v -2553.83 1104.24 810 v -2553.83 1104.24 510 v -2553.83 1104.24 810 v -2557.07 1102.07 810 v -2553.83 1104.24 510 v -2553.83 1104.24 510 v -2557.07 1102.07 810 v -2557.07 1102.07 510 v -2557.07 1102.07 810 v -2559.24 1098.83 810 v -2557.07 1102.07 510 v -2557.07 1102.07 510 v -2559.24 1098.83 810 v -2559.24 1098.83 510 v -2559.24 1098.83 810 v -2560 1095 810 v -2559.24 1098.83 510 v -2559.24 1098.83 510 v -2560 1095 810 v -2560 1095 510 v -2460 1095 810 v -2459.24 1091.17 810 v -2460 1095 510 v -2460 1095 510 v -2459.24 1091.17 810 v -2459.24 1091.17 510 v -2459.24 1091.17 810 v -2457.07 1087.93 810 v -2459.24 1091.17 510 v -2459.24 1091.17 510 v -2457.07 1087.93 810 v -2457.07 1087.93 510 v -2457.07 1087.93 810 v -2453.83 1085.76 810 v -2457.07 1087.93 510 v -2457.07 1087.93 510 v -2453.83 1085.76 810 v -2453.83 1085.76 510 v -2453.83 1085.76 810 v -2450 1085 810 v -2453.83 1085.76 510 v -2453.83 1085.76 510 v -2450 1085 810 v -2450 1085 510 v -2450 1085 810 v -2446.17 1085.76 810 v -2450 1085 510 v -2450 1085 510 v -2446.17 1085.76 810 v -2446.17 1085.76 510 v -2446.17 1085.76 810 v -2442.93 1087.93 810 v -2446.17 1085.76 510 v -2446.17 1085.76 510 v -2442.93 1087.93 810 v -2442.93 1087.93 510 v -2442.93 1087.93 810 v -2440.76 1091.17 810 v -2442.93 1087.93 510 v -2442.93 1087.93 510 v -2440.76 1091.17 810 v -2440.76 1091.17 510 v -2440.76 1091.17 810 v -2440 1095 810 v -2440.76 1091.17 510 v -2440.76 1091.17 510 v -2440 1095 810 v -2440 1095 510 v -2440 1095 810 v -2440.76 1098.83 810 v -2440 1095 510 v -2440 1095 510 v -2440.76 1098.83 810 v -2440.76 1098.83 510 v -2440.76 1098.83 810 v -2442.93 1102.07 810 v -2440.76 1098.83 510 v -2440.76 1098.83 510 v -2442.93 1102.07 810 v -2442.93 1102.07 510 v -2442.93 1102.07 810 v -2446.17 1104.24 810 v -2442.93 1102.07 510 v -2442.93 1102.07 510 v -2446.17 1104.24 810 v -2446.17 1104.24 510 v -2446.17 1104.24 810 v -2450 1105 810 v -2446.17 1104.24 510 v -2446.17 1104.24 510 v -2450 1105 810 v -2450 1105 510 v -2450 1105 810 v -2453.83 1104.24 810 v -2450 1105 510 v -2450 1105 510 v -2453.83 1104.24 810 v -2453.83 1104.24 510 v -2453.83 1104.24 810 v -2457.07 1102.07 810 v -2453.83 1104.24 510 v -2453.83 1104.24 510 v -2457.07 1102.07 810 v -2457.07 1102.07 510 v -2457.07 1102.07 810 v -2459.24 1098.83 810 v -2457.07 1102.07 510 v -2457.07 1102.07 510 v -2459.24 1098.83 810 v -2459.24 1098.83 510 v -2459.24 1098.83 810 v -2460 1095 810 v -2459.24 1098.83 510 v -2459.24 1098.83 510 v -2460 1095 810 v -2460 1095 510 v -2300 1110 910 v -2700 1110 910 v -2300 1100 910 v -2300 1100 910 v -2700 1110 910 v -2700 1100 910 v -2700 1110 710 v -2300 1110 710 v -2700 1100 710 v -2700 1100 710 v -2300 1110 710 v -2300 1100 710 v -2700 1110 910 v -2700 1110 710 v -2700 1100 910 v -2700 1100 910 v -2700 1110 710 v -2700 1100 710 v -2300 1110 710 v -2300 1110 910 v -2300 1100 710 v -2300 1100 710 v -2300 1110 910 v -2300 1100 910 v -2300 1100 710 v -2300 1100 910 v -2700 1100 710 v -2700 1100 710 v -2300 1100 910 v -2700 1100 910 v -2300 1110 710 v -2700 1110 710 v -2300 1110 910 v -2300 1110 910 v -2700 1110 710 v -2700 1110 910 f 1 2 3 f 4 5 6 f 7 8 9 f 10 11 12 f 13 14 15 f 16 17 18 f 19 20 21 f 22 23 24 f 25 26 27 f 28 29 30 f 31 32 33 f 34 35 36 f 37 38 39 f 40 41 42 f 43 44 45 f 46 47 48 f 49 50 51 f 52 53 54 f 55 56 57 f 58 59 60 f 61 62 63 f 64 65 66 f 67 68 69 f 70 71 72 f 73 74 75 f 76 77 78 f 79 80 81 f 82 83 84 f 85 86 87 f 88 89 90 f 91 92 93 f 94 95 96 f 97 98 99 f 100 101 102 f 103 104 105 f 106 107 108 f 109 110 111 f 112 113 114 f 115 116 117 f 118 119 120 f 121 122 123 f 124 125 126 f 127 128 129 f 130 131 132 f 133 134 135 f 136 137 138 f 139 140 141 f 142 143 144 f 145 146 147 f 148 149 150 f 151 152 153 f 154 155 156 f 157 158 159 f 160 161 162 f 163 164 165 f 166 167 168 f 169 170 171 f 172 173 174 f 175 176 177 f 178 179 180 f 181 182 183 f 184 185 186 f 187 188 189 f 190 191 192 f 193 194 195 f 196 197 198 f 199 200 201 f 202 203 204 f 205 206 207 f 208 209 210 f 211 212 213 f 214 215 216 f 217 218 219 f 220 221 222 f 223 224 225 f 226 227 228 f 229 230 231 f 232 233 234 f 235 236 237 f 238 239 240 f 241 242 243 f 244 245 246 f 247 248 249 f 250 251 252 f 253 254 255 f 256 257 258 f 259 260 261 f 262 263 264 f 265 266 267 f 268 269 270 f 271 272 273 f 274 275 276 f 277 278 279 f 280 281 282 f 283 284 285 f 286 287 288 f 289 290 291 f 292 293 294 f 295 296 297 f 298 299 300 f 301 302 303 f 304 305 306 f 307 308 309 f 310 311 312 f 313 314 315 f 316 317 318 f 319 320 321 f 322 323 324 f 325 326 327 f 328 329 330 f 331 332 333 f 334 335 336 f 337 338 339 f 340 341 342 f 343 344 345 f 346 347 348 f 349 350 351 f 352 353 354 f 355 356 357 f 358 359 360 f 361 362 363 f 364 365 366 f 367 368 369 f 370 371 372 f 373 374 375 f 376 377 378 f 379 380 381 f 382 383 384 f 385 386 387 f 388 389 390 f 391 392 393 f 394 395 396 f 397 398 399 f 400 401 402 f 403 404 405 f 406 407 408 f 409 410 411 f 412 413 414 f 415 416 417 f 418 419 420 f 421 422 423 f 424 425 426 f 427 428 429 f 430 431 432 f 433 434 435 f 436 437 438 f 439 440 441 f 442 443 444 f 445 446 447 f 448 449 450 f 451 452 453 f 454 455 456 f 457 458 459 f 460 461 462 f 463 464 465 f 466 467 468 f 469 470 471 f 472 473 474 f 475 476 477 f 478 479 480 f 481 482 483 f 484 485 486 f 487 488 489 f 490 491 492 f 493 494 495 f 496 497 498 f 499 500 501 f 502 503 504 f 505 506 507 f 508 509 510 f 511 512 513 f 514 515 516 f 517 518 519 f 520 521 522 f 523 524 525 f 526 527 528 f 529 530 531 f 532 533 534 f 535 536 537 f 538 539 540 f 541 542 543 f 544 545 546 f 547 548 549 f 550 551 552 f 553 554 555 f 556 557 558 f 559 560 561 f 562 563 564 f 565 566 567 f 568 569 570 f 571 572 573 f 574 575 576 f 577 578 579 f 580 581 582 f 583 584 585 f 586 587 588 f 589 590 591 f 592 593 594 f 595 596 597 f 598 599 600 f 601 602 603 f 604 605 606 f 607 608 609 f 610 611 612 f 613 614 615 f 616 617 618 f 619 620 621 f 622 623 624 f 625 626 627 f 628 629 630 f 631 632 633 f 634 635 636 f 637 638 639 f 640 641 642 f 643 644 645 f 646 647 648 f 649 650 651 f 652 653 654 f 655 656 657 f 658 659 660 f 661 662 663 f 664 665 666 f 667 668 669 f 670 671 672 f 673 674 675 f 676 677 678 f 679 680 681 f 682 683 684 f 685 686 687 f 688 689 690 f 691 692 693 f 694 695 696 f 697 698 699 f 700 701 702 f 703 704 705 f 706 707 708 f 709 710 711 f 712 713 714 f 715 716 717 f 718 719 720 f 721 722 723 f 724 725 726 f 727 728 729 f 730 731 732 f 733 734 735 f 736 737 738 f 739 740 741 f 742 743 744 f 745 746 747 f 748 749 750 f 751 752 753 f 754 755 756 f 757 758 759 f 760 761 762 f 763 764 765 f 766 767 768 f 769 770 771 f 772 773 774 f 775 776 777 f 778 779 780 f 781 782 783 f 784 785 786 f 787 788 789 f 790 791 792 f 793 794 795 f 796 797 798 f 799 800 801 f 802 803 804 f 805 806 807 f 808 809 810 f 811 812 813 f 814 815 816 f 817 818 819 f 820 821 822 f 823 824 825 f 826 827 828 f 829 830 831 f 832 833 834 f 835 836 837 f 838 839 840 f 841 842 843 f 844 845 846 f 847 848 849 f 850 851 852 f 853 854 855 f 856 857 858 f 859 860 861 f 862 863 864 f 865 866 867 f 868 869 870 f 871 872 873 f 874 875 876 f 877 878 879 f 880 881 882 f 883 884 885 f 886 887 888 f 889 890 891 f 892 893 894 f 895 896 897 f 898 899 900 f 901 902 903 f 904 905 906 f 907 908 909 f 910 911 912 f 913 914 915 f 916 917 918 f 919 920 921 f 922 923 924 f 925 926 927 f 928 929 930 f 931 932 933 f 934 935 936 f 937 938 939 f 940 941 942 f 943 944 945 f 946 947 948 f 949 950 951 f 952 953 954 f 955 956 957 f 958 959 960 f 961 962 963 f 964 965 966 f 967 968 969 f 970 971 972 f 973 974 975 f 976 977 978 f 979 980 981 f 982 983 984 f 985 986 987 f 988 989 990 f 991 992 993 f 994 995 996 f 997 998 999 f 1000 1001 1002 f 1003 1004 1005 f 1006 1007 1008 f 1009 1010 1011 f 1012 1013 1014 f 1015 1016 1017 f 1018 1019 1020 f 1021 1022 1023 f 1024 1025 1026 f 1027 1028 1029 f 1030 1031 1032 f 1033 1034 1035 f 1036 1037 1038 f 1039 1040 1041 f 1042 1043 1044 f 1045 1046 1047 f 1048 1049 1050 f 1051 1052 1053 f 1054 1055 1056 f 1057 1058 1059 f 1060 1061 1062 f 1063 1064 1065 f 1066 1067 1068 f 1069 1070 1071 f 1072 1073 1074 f 1075 1076 1077 f 1078 1079 1080 f 1081 1082 1083 f 1084 1085 1086 f 1087 1088 1089 f 1090 1091 1092 f 1093 1094 1095 f 1096 1097 1098 f 1099 1100 1101 f 1102 1103 1104 f 1105 1106 1107 f 1108 1109 1110 f 1111 1112 1113 f 1114 1115 1116 f 1117 1118 1119 f 1120 1121 1122 f 1123 1124 1125 f 1126 1127 1128 f 1129 1130 1131 f 1132 1133 1134 f 1135 1136 1137 f 1138 1139 1140 f 1141 1142 1143 f 1144 1145 1146 f 1147 1148 1149 f 1150 1151 1152 f 1153 1154 1155 f 1156 1157 1158 f 1159 1160 1161 f 1162 1163 1164 f 1165 1166 1167 f 1168 1169 1170 f 1171 1172 1173 f 1174 1175 1176 f 1177 1178 1179 f 1180 1181 1182 f 1183 1184 1185 f 1186 1187 1188 f 1189 1190 1191 f 1192 1193 1194 f 1195 1196 1197 f 1198 1199 1200 f 1201 1202 1203 f 1204 1205 1206 f 1207 1208 1209 f 1210 1211 1212 f 1213 1214 1215 f 1216 1217 1218 f 1219 1220 1221 f 1222 1223 1224 f 1225 1226 1227 f 1228 1229 1230 f 1231 1232 1233 f 1234 1235 1236 f 1237 1238 1239 f 1240 1241 1242 f 1243 1244 1245 f 1246 1247 1248 f 1249 1250 1251 f 1252 1253 1254 f 1255 1256 1257 f 1258 1259 1260 f 1261 1262 1263 f 1264 1265 1266 f 1267 1268 1269 f 1270 1271 1272 f 1273 1274 1275 f 1276 1277 1278 f 1279 1280 1281 f 1282 1283 1284 f 1285 1286 1287 f 1288 1289 1290 f 1291 1292 1293 f 1294 1295 1296 f 1297 1298 1299 f 1300 1301 1302 f 1303 1304 1305 f 1306 1307 1308 f 1309 1310 1311 f 1312 1313 1314 f 1315 1316 1317 f 1318 1319 1320 f 1321 1322 1323 f 1324 1325 1326 f 1327 1328 1329 f 1330 1331 1332 f 1333 1334 1335 f 1336 1337 1338 f 1339 1340 1341 f 1342 1343 1344 f 1345 1346 1347 f 1348 1349 1350 f 1351 1352 1353 f 1354 1355 1356 f 1357 1358 1359 f 1360 1361 1362 f 1363 1364 1365 f 1366 1367 1368 f 1369 1370 1371 f 1372 1373 1374 f 1375 1376 1377 f 1378 1379 1380 f 1381 1382 1383 f 1384 1385 1386 f 1387 1388 1389 f 1390 1391 1392 f 1393 1394 1395 f 1396 1397 1398 f 1399 1400 1401 f 1402 1403 1404 f 1405 1406 1407 f 1408 1409 1410 f 1411 1412 1413 f 1414 1415 1416 f 1417 1418 1419 f 1420 1421 1422 f 1423 1424 1425 f 1426 1427 1428 f 1429 1430 1431 f 1432 1433 1434 f 1435 1436 1437 f 1438 1439 1440 f 1441 1442 1443 f 1444 1445 1446 f 1447 1448 1449 f 1450 1451 1452 f 1453 1454 1455 f 1456 1457 1458 f 1459 1460 1461 f 1462 1463 1464 f 1465 1466 1467 f 1468 1469 1470 f 1471 1472 1473 f 1474 1475 1476 f 1477 1478 1479 f 1480 1481 1482 f 1483 1484 1485 f 1486 1487 1488 f 1489 1490 1491 f 1492 1493 1494 f 1495 1496 1497 f 1498 1499 1500 f 1501 1502 1503 f 1504 1505 1506 f 1507 1508 1509 f 1510 1511 1512 f 1513 1514 1515 f 1516 1517 1518 f 1519 1520 1521 f 1522 1523 1524 f 1525 1526 1527 f 1528 1529 1530 f 1531 1532 1533 f 1534 1535 1536 f 1537 1538 1539 f 1540 1541 1542 f 1543 1544 1545 f 1546 1547 1548 f 1549 1550 1551 f 1552 1553 1554 f 1555 1556 1557 f 1558 1559 1560 f 1561 1562 1563 f 1564 1565 1566 f 1567 1568 1569 f 1570 1571 1572 f 1573 1574 1575 f 1576 1577 1578 f 1579 1580 1581 f 1582 1583 1584 f 1585 1586 1587 f 1588 1589 1590 f 1591 1592 1593 f 1594 1595 1596 f 1597 1598 1599 f 1600 1601 1602 f 1603 1604 1605 f 1606 1607 1608 f 1609 1610 1611 f 1612 1613 1614 f 1615 1616 1617 f 1618 1619 1620 f 1621 1622 1623 f 1624 1625 1626 f 1627 1628 1629 f 1630 1631 1632 f 1633 1634 1635 f 1636 1637 1638 f 1639 1640 1641 f 1642 1643 1644 f 1645 1646 1647 f 1648 1649 1650 f 1651 1652 1653 f 1654 1655 1656 f 1657 1658 1659 f 1660 1661 1662 f 1663 1664 1665 f 1666 1667 1668 f 1669 1670 1671 f 1672 1673 1674 f 1675 1676 1677 f 1678 1679 1680 f 1681 1682 1683 f 1684 1685 1686 f 1687 1688 1689 f 1690 1691 1692 f 1693 1694 1695 f 1696 1697 1698 f 1699 1700 1701 f 1702 1703 1704 f 1705 1706 1707 f 1708 1709 1710 f 1711 1712 1713 f 1714 1715 1716 f 1717 1718 1719 f 1720 1721 1722 f 1723 1724 1725 f 1726 1727 1728 f 1729 1730 1731 f 1732 1733 1734 f 1735 1736 1737 f 1738 1739 1740 f 1741 1742 1743 f 1744 1745 1746 f 1747 1748 1749 f 1750 1751 1752 f 1753 1754 1755 f 1756 1757 1758 f 1759 1760 1761 f 1762 1763 1764 f 1765 1766 1767 f 1768 1769 1770 f 1771 1772 1773 f 1774 1775 1776 f 1777 1778 1779 f 1780 1781 1782 f 1783 1784 1785 f 1786 1787 1788 f 1789 1790 1791 f 1792 1793 1794 f 1795 1796 1797 f 1798 1799 1800 f 1801 1802 1803 f 1804 1805 1806 f 1807 1808 1809 f 1810 1811 1812 f 1813 1814 1815 f 1816 1817 1818 f 1819 1820 1821 f 1822 1823 1824 f 1825 1826 1827 f 1828 1829 1830 f 1831 1832 1833 f 1834 1835 1836 f 1837 1838 1839 f 1840 1841 1842 f 1843 1844 1845 f 1846 1847 1848 f 1849 1850 1851 f 1852 1853 1854 f 1855 1856 1857 f 1858 1859 1860 f 1861 1862 1863 f 1864 1865 1866 f 1867 1868 1869 f 1870 1871 1872 f 1873 1874 1875 f 1876 1877 1878 f 1879 1880 1881 f 1882 1883 1884 f 1885 1886 1887 f 1888 1889 1890 f 1891 1892 1893 f 1894 1895 1896 f 1897 1898 1899 f 1900 1901 1902 f 1903 1904 1905 f 1906 1907 1908 f 1909 1910 1911 f 1912 1913 1914 f 1915 1916 1917 f 1918 1919 1920 f 1921 1922 1923 f 1924 1925 1926 f 1927 1928 1929 f 1930 1931 1932 f 1933 1934 1935 f 1936 1937 1938 f 1939 1940 1941 f 1942 1943 1944 f 1945 1946 1947 f 1948 1949 1950 f 1951 1952 1953 f 1954 1955 1956 f 1957 1958 1959 f 1960 1961 1962 f 1963 1964 1965 f 1966 1967 1968 f 1969 1970 1971 f 1972 1973 1974 f 1975 1976 1977 f 1978 1979 1980 f 1981 1982 1983 f 1984 1985 1986 f 1987 1988 1989 f 1990 1991 1992 f 1993 1994 1995 f 1996 1997 1998 f 1999 2000 2001 f 2002 2003 2004 f 2005 2006 2007 f 2008 2009 2010 f 2011 2012 2013 f 2014 2015 2016 f 2017 2018 2019 f 2020 2021 2022 f 2023 2024 2025 f 2026 2027 2028 f 2029 2030 2031 f 2032 2033 2034 f 2035 2036 2037 f 2038 2039 2040 f 2041 2042 2043 f 2044 2045 2046 f 2047 2048 2049 f 2050 2051 2052 f 2053 2054 2055 f 2056 2057 2058 f 2059 2060 2061 f 2062 2063 2064 f 2065 2066 2067 f 2068 2069 2070 f 2071 2072 2073 f 2074 2075 2076 f 2077 2078 2079 f 2080 2081 2082 f 2083 2084 2085 f 2086 2087 2088 f 2089 2090 2091 f 2092 2093 2094 f 2095 2096 2097 f 2098 2099 2100 f 2101 2102 2103 f 2104 2105 2106 f 2107 2108 2109 f 2110 2111 2112 f 2113 2114 2115 f 2116 2117 2118 f 2119 2120 2121 f 2122 2123 2124 f 2125 2126 2127 f 2128 2129 2130 f 2131 2132 2133 f 2134 2135 2136 f 2137 2138 2139 f 2140 2141 2142 f 2143 2144 2145 f 2146 2147 2148 f 2149 2150 2151 f 2152 2153 2154 f 2155 2156 2157 f 2158 2159 2160 f 2161 2162 2163 f 2164 2165 2166 f 2167 2168 2169 f 2170 2171 2172 f 2173 2174 2175 f 2176 2177 2178 f 2179 2180 2181 f 2182 2183 2184 f 2185 2186 2187 f 2188 2189 2190 f 2191 2192 2193 f 2194 2195 2196 f 2197 2198 2199 f 2200 2201 2202 f 2203 2204 2205 f 2206 2207 2208 f 2209 2210 2211 f 2212 2213 2214 f 2215 2216 2217 f 2218 2219 2220 f 2221 2222 2223 f 2224 2225 2226 f 2227 2228 2229 f 2230 2231 2232 f 2233 2234 2235 f 2236 2237 2238 f 2239 2240 2241 f 2242 2243 2244 f 2245 2246 2247 f 2248 2249 2250 f 2251 2252 2253 f 2254 2255 2256 f 2257 2258 2259 f 2260 2261 2262 f 2263 2264 2265 f 2266 2267 2268 f 2269 2270 2271 f 2272 2273 2274 f 2275 2276 2277 f 2278 2279 2280 f 2281 2282 2283 f 2284 2285 2286 f 2287 2288 2289 f 2290 2291 2292 f 2293 2294 2295 f 2296 2297 2298 f 2299 2300 2301 f 2302 2303 2304 f 2305 2306 2307 f 2308 2309 2310 f 2311 2312 2313 f 2314 2315 2316 f 2317 2318 2319 f 2320 2321 2322 f 2323 2324 2325 f 2326 2327 2328 f 2329 2330 2331 f 2332 2333 2334 f 2335 2336 2337 f 2338 2339 2340 f 2341 2342 2343 f 2344 2345 2346 f 2347 2348 2349 f 2350 2351 2352 f 2353 2354 2355 f 2356 2357 2358 f 2359 2360 2361 f 2362 2363 2364 f 2365 2366 2367 f 2368 2369 2370 f 2371 2372 2373 f 2374 2375 2376 f 2377 2378 2379 f 2380 2381 2382 f 2383 2384 2385 f 2386 2387 2388 f 2389 2390 2391 f 2392 2393 2394 f 2395 2396 2397 f 2398 2399 2400 f 2401 2402 2403 f 2404 2405 2406 f 2407 2408 2409 f 2410 2411 2412 f 2413 2414 2415 f 2416 2417 2418 f 2419 2420 2421 f 2422 2423 2424 f 2425 2426 2427 f 2428 2429 2430 f 2431 2432 2433 f 2434 2435 2436 f 2437 2438 2439 f 2440 2441 2442 f 2443 2444 2445 f 2446 2447 2448 f 2449 2450 2451 f 2452 2453 2454 f 2455 2456 2457 f 2458 2459 2460 f 2461 2462 2463 f 2464 2465 2466 f 2467 2468 2469 f 2470 2471 2472 f 2473 2474 2475 f 2476 2477 2478 f 2479 2480 2481 f 2482 2483 2484 f 2485 2486 2487 f 2488 2489 2490 f 2491 2492 2493 f 2494 2495 2496 f 2497 2498 2499 f 2500 2501 2502 f 2503 2504 2505 f 2506 2507 2508 f 2509 2510 2511 f 2512 2513 2514 f 2515 2516 2517 f 2518 2519 2520 f 2521 2522 2523 f 2524 2525 2526 f 2527 2528 2529 f 2530 2531 2532 f 2533 2534 2535 f 2536 2537 2538 f 2539 2540 2541 f 2542 2543 2544 f 2545 2546 2547 f 2548 2549 2550 f 2551 2552 2553 f 2554 2555 2556 f 2557 2558 2559 f 2560 2561 2562 f 2563 2564 2565 f 2566 2567 2568 f 2569 2570 2571 f 2572 2573 2574 f 2575 2576 2577 f 2578 2579 2580 f 2581 2582 2583 f 2584 2585 2586 f 2587 2588 2589 f 2590 2591 2592 f 2593 2594 2595 f 2596 2597 2598 f 2599 2600 2601 f 2602 2603 2604 f 2605 2606 2607 f 2608 2609 2610 f 2611 2612 2613 f 2614 2615 2616 f 2617 2618 2619 f 2620 2621 2622 f 2623 2624 2625 f 2626 2627 2628 f 2629 2630 2631 f 2632 2633 2634 f 2635 2636 2637 f 2638 2639 2640 f 2641 2642 2643 f 2644 2645 2646 f 2647 2648 2649 f 2650 2651 2652 f 2653 2654 2655 f 2656 2657 2658 f 2659 2660 2661 f 2662 2663 2664 f 2665 2666 2667 f 2668 2669 2670 f 2671 2672 2673 f 2674 2675 2676 f 2677 2678 2679 f 2680 2681 2682 f 2683 2684 2685 f 2686 2687 2688 f 2689 2690 2691 f 2692 2693 2694 f 2695 2696 2697 f 2698 2699 2700 f 2701 2702 2703 f 2704 2705 2706 f 2707 2708 2709 f 2710 2711 2712 f 2713 2714 2715 f 2716 2717 2718 f 2719 2720 2721 f 2722 2723 2724 f 2725 2726 2727 f 2728 2729 2730 f 2731 2732 2733 f 2734 2735 2736 f 2737 2738 2739 f 2740 2741 2742 f 2743 2744 2745 f 2746 2747 2748 f 2749 2750 2751 f 2752 2753 2754 f 2755 2756 2757 f 2758 2759 2760 f 2761 2762 2763 f 2764 2765 2766 f 2767 2768 2769 f 2770 2771 2772 f 2773 2774 2775 f 2776 2777 2778 f 2779 2780 2781 f 2782 2783 2784 f 2785 2786 2787 f 2788 2789 2790 f 2791 2792 2793 f 2794 2795 2796 f 2797 2798 2799 f 2800 2801 2802 f 2803 2804 2805 f 2806 2807 2808 f 2809 2810 2811 f 2812 2813 2814 f 2815 2816 2817 f 2818 2819 2820 f 2821 2822 2823 f 2824 2825 2826 f 2827 2828 2829 f 2830 2831 2832 f 2833 2834 2835 f 2836 2837 2838 f 2839 2840 2841 f 2842 2843 2844 f 2845 2846 2847 f 2848 2849 2850 f 2851 2852 2853 f 2854 2855 2856 f 2857 2858 2859 f 2860 2861 2862 f 2863 2864 2865 f 2866 2867 2868 f 2869 2870 2871 f 2872 2873 2874 f 2875 2876 2877 f 2878 2879 2880 f 2881 2882 2883 f 2884 2885 2886 f 2887 2888 2889 f 2890 2891 2892 f 2893 2894 2895 f 2896 2897 2898 f 2899 2900 2901 f 2902 2903 2904 f 2905 2906 2907 f 2908 2909 2910 f 2911 2912 2913 f 2914 2915 2916 f 2917 2918 2919 f 2920 2921 2922 f 2923 2924 2925 f 2926 2927 2928 f 2929 2930 2931 f 2932 2933 2934 f 2935 2936 2937 f 2938 2939 2940 f 2941 2942 2943 f 2944 2945 2946 f 2947 2948 2949 f 2950 2951 2952 f 2953 2954 2955 f 2956 2957 2958 f 2959 2960 2961 f 2962 2963 2964 f 2965 2966 2967 f 2968 2969 2970 f 2971 2972 2973 f 2974 2975 2976 f 2977 2978 2979 f 2980 2981 2982 f 2983 2984 2985 f 2986 2987 2988 f 2989 2990 2991 f 2992 2993 2994 f 2995 2996 2997 f 2998 2999 3000 f 3001 3002 3003 f 3004 3005 3006 f 3007 3008 3009 f 3010 3011 3012 f 3013 3014 3015 f 3016 3017 3018 f 3019 3020 3021 f 3022 3023 3024 f 3025 3026 3027 f 3028 3029 3030 f 3031 3032 3033 f 3034 3035 3036 f 3037 3038 3039 f 3040 3041 3042 f 3043 3044 3045 f 3046 3047 3048 f 3049 3050 3051 f 3052 3053 3054 f 3055 3056 3057 f 3058 3059 3060 f 3061 3062 3063 f 3064 3065 3066 f 3067 3068 3069 f 3070 3071 3072 f 3073 3074 3075 f 3076 3077 3078 f 3079 3080 3081 f 3082 3083 3084 f 3085 3086 3087 f 3088 3089 3090 f 3091 3092 3093 f 3094 3095 3096 f 3097 3098 3099 f 3100 3101 3102 f 3103 3104 3105 f 3106 3107 3108 f 3109 3110 3111 f 3112 3113 3114 f 3115 3116 3117 f 3118 3119 3120 f 3121 3122 3123 f 3124 3125 3126 f 3127 3128 3129 f 3130 3131 3132 f 3133 3134 3135 f 3136 3137 3138 f 3139 3140 3141 f 3142 3143 3144 f 3145 3146 3147 f 3148 3149 3150 f 3151 3152 3153 f 3154 3155 3156 f 3157 3158 3159 f 3160 3161 3162 f 3163 3164 3165 f 3166 3167 3168 f 3169 3170 3171 f 3172 3173 3174 f 3175 3176 3177 f 3178 3179 3180 f 3181 3182 3183 f 3184 3185 3186 f 3187 3188 3189 f 3190 3191 3192 f 3193 3194 3195 f 3196 3197 3198 f 3199 3200 3201 f 3202 3203 3204 f 3205 3206 3207 f 3208 3209 3210 f 3211 3212 3213 f 3214 3215 3216 f 3217 3218 3219 f 3220 3221 3222 f 3223 3224 3225 f 3226 3227 3228 f 3229 3230 3231 f 3232 3233 3234 f 3235 3236 3237 f 3238 3239 3240 f 3241 3242 3243 f 3244 3245 3246 f 3247 3248 3249 f 3250 3251 3252 f 3253 3254 3255 f 3256 3257 3258 f 3259 3260 3261 f 3262 3263 3264 f 3265 3266 3267 f 3268 3269 3270 f 3271 3272 3273 f 3274 3275 3276 f 3277 3278 3279 f 3280 3281 3282 f 3283 3284 3285 f 3286 3287 3288 f 3289 3290 3291 f 3292 3293 3294 f 3295 3296 3297 f 3298 3299 3300 f 3301 3302 3303 f 3304 3305 3306 f 3307 3308 3309 f 3310 3311 3312 f 3313 3314 3315 f 3316 3317 3318 f 3319 3320 3321 f 3322 3323 3324 f 3325 3326 3327 f 3328 3329 3330 f 3331 3332 3333 f 3334 3335 3336 f 3337 3338 3339 f 3340 3341 3342 f 3343 3344 3345 f 3346 3347 3348 f 3349 3350 3351 f 3352 3353 3354 f 3355 3356 3357 f 3358 3359 3360 f 3361 3362 3363 f 3364 3365 3366 f 3367 3368 3369 f 3370 3371 3372 f 3373 3374 3375 f 3376 3377 3378 f 3379 3380 3381 f 3382 3383 3384 f 3385 3386 3387 f 3388 3389 3390 f 3391 3392 3393 f 3394 3395 3396 f 3397 3398 3399 f 3400 3401 3402 f 3403 3404 3405 f 3406 3407 3408 f 3409 3410 3411 f 3412 3413 3414 f 3415 3416 3417 f 3418 3419 3420 f 3421 3422 3423 f 3424 3425 3426 f 3427 3428 3429 f 3430 3431 3432 f 3433 3434 3435 f 3436 3437 3438 f 3439 3440 3441 f 3442 3443 3444 f 3445 3446 3447 f 3448 3449 3450 f 3451 3452 3453 f 3454 3455 3456 f 3457 3458 3459 f 3460 3461 3462 f 3463 3464 3465 f 3466 3467 3468 f 3469 3470 3471 f 3472 3473 3474 f 3475 3476 3477 f 3478 3479 3480 f 3481 3482 3483 f 3484 3485 3486 f 3487 3488 3489 f 3490 3491 3492 f 3493 3494 3495 f 3496 3497 3498 f 3499 3500 3501 f 3502 3503 3504 f 3505 3506 3507 f 3508 3509 3510 f 3511 3512 3513 f 3514 3515 3516 f 3517 3518 3519 f 3520 3521 3522 f 3523 3524 3525 f 3526 3527 3528 f 3529 3530 3531 f 3532 3533 3534 f 3535 3536 3537 f 3538 3539 3540 f 3541 3542 3543 f 3544 3545 3546 f 3547 3548 3549 f 3550 3551 3552 f 3553 3554 3555 f 3556 3557 3558 f 3559 3560 3561 f 3562 3563 3564 f 3565 3566 3567 f 3568 3569 3570 f 3571 3572 3573 f 3574 3575 3576 f 3577 3578 3579 f 3580 3581 3582 f 3583 3584 3585 f 3586 3587 3588 f 3589 3590 3591 f 3592 3593 3594 f 3595 3596 3597 f 3598 3599 3600 f 3601 3602 3603 f 3604 3605 3606 f 3607 3608 3609 f 3610 3611 3612 f 3613 3614 3615 f 3616 3617 3618 f 3619 3620 3621 f 3622 3623 3624 f 3625 3626 3627 f 3628 3629 3630 f 3631 3632 3633 f 3634 3635 3636 f 3637 3638 3639 f 3640 3641 3642 f 3643 3644 3645 f 3646 3647 3648 f 3649 3650 3651 f 3652 3653 3654 f 3655 3656 3657 f 3658 3659 3660 f 3661 3662 3663 f 3664 3665 3666 f 3667 3668 3669 f 3670 3671 3672 f 3673 3674 3675 f 3676 3677 3678 f 3679 3680 3681 f 3682 3683 3684 f 3685 3686 3687 f 3688 3689 3690 f 3691 3692 3693 f 3694 3695 3696 f 3697 3698 3699 f 3700 3701 3702 f 3703 3704 3705 f 3706 3707 3708 f 3709 3710 3711 f 3712 3713 3714 f 3715 3716 3717 f 3718 3719 3720 f 3721 3722 3723 f 3724 3725 3726 f 3727 3728 3729 f 3730 3731 3732 f 3733 3734 3735 f 3736 3737 3738 f 3739 3740 3741 f 3742 3743 3744 f 3745 3746 3747 f 3748 3749 3750 f 3751 3752 3753 f 3754 3755 3756 f 3757 3758 3759 f 3760 3761 3762 f 3763 3764 3765 f 3766 3767 3768 f 3769 3770 3771 f 3772 3773 3774 f 3775 3776 3777 f 3778 3779 3780 f 3781 3782 3783 f 3784 3785 3786 f 3787 3788 3789 f 3790 3791 3792 f 3793 3794 3795 f 3796 3797 3798 f 3799 3800 3801 f 3802 3803 3804 f 3805 3806 3807 f 3808 3809 3810 f 3811 3812 3813 f 3814 3815 3816 f 3817 3818 3819 f 3820 3821 3822 f 3823 3824 3825 f 3826 3827 3828 f 3829 3830 3831 f 3832 3833 3834 f 3835 3836 3837 f 3838 3839 3840 f 3841 3842 3843 f 3844 3845 3846 f 3847 3848 3849 f 3850 3851 3852 f 3853 3854 3855 f 3856 3857 3858 f 3859 3860 3861 f 3862 3863 3864 f 3865 3866 3867 f 3868 3869 3870 f 3871 3872 3873 f 3874 3875 3876 f 3877 3878 3879 f 3880 3881 3882 f 3883 3884 3885 f 3886 3887 3888 f 3889 3890 3891 f 3892 3893 3894 f 3895 3896 3897 f 3898 3899 3900 f 3901 3902 3903 f 3904 3905 3906 f 3907 3908 3909 f 3910 3911 3912 f 3913 3914 3915 f 3916 3917 3918 f 3919 3920 3921 f 3922 3923 3924 f 3925 3926 3927 f 3928 3929 3930 f 3931 3932 3933 f 3934 3935 3936 f 3937 3938 3939 f 3940 3941 3942 f 3943 3944 3945 f 3946 3947 3948 f 3949 3950 3951 f 3952 3953 3954 f 3955 3956 3957 f 3958 3959 3960 f 3961 3962 3963 f 3964 3965 3966 f 3967 3968 3969 f 3970 3971 3972 f 3973 3974 3975 f 3976 3977 3978 f 3979 3980 3981 f 3982 3983 3984 f 3985 3986 3987 f 3988 3989 3990 f 3991 3992 3993 f 3994 3995 3996 f 3997 3998 3999 f 4000 4001 4002 f 4003 4004 4005 f 4006 4007 4008 f 4009 4010 4011 f 4012 4013 4014 f 4015 4016 4017 f 4018 4019 4020 f 4021 4022 4023 f 4024 4025 4026 f 4027 4028 4029 f 4030 4031 4032 f 4033 4034 4035 f 4036 4037 4038 f 4039 4040 4041 f 4042 4043 4044 f 4045 4046 4047 f 4048 4049 4050 f 4051 4052 4053 f 4054 4055 4056 f 4057 4058 4059 f 4060 4061 4062 f 4063 4064 4065 f 4066 4067 4068 f 4069 4070 4071 f 4072 4073 4074 f 4075 4076 4077 f 4078 4079 4080 f 4081 4082 4083 f 4084 4085 4086 f 4087 4088 4089 f 4090 4091 4092 f 4093 4094 4095 f 4096 4097 4098 f 4099 4100 4101 f 4102 4103 4104 f 4105 4106 4107 f 4108 4109 4110 f 4111 4112 4113 f 4114 4115 4116 f 4117 4118 4119 f 4120 4121 4122 f 4123 4124 4125 f 4126 4127 4128 f 4129 4130 4131 f 4132 4133 4134 f 4135 4136 4137 f 4138 4139 4140 f 4141 4142 4143 f 4144 4145 4146 f 4147 4148 4149 f 4150 4151 4152 f 4153 4154 4155 f 4156 4157 4158 f 4159 4160 4161 f 4162 4163 4164 f 4165 4166 4167 f 4168 4169 4170 f 4171 4172 4173 f 4174 4175 4176 f 4177 4178 4179 f 4180 4181 4182 f 4183 4184 4185 f 4186 4187 4188 f 4189 4190 4191 f 4192 4193 4194 f 4195 4196 4197 f 4198 4199 4200 f 4201 4202 4203 f 4204 4205 4206 f 4207 4208 4209 f 4210 4211 4212 f 4213 4214 4215 f 4216 4217 4218 f 4219 4220 4221 f 4222 4223 4224 f 4225 4226 4227 f 4228 4229 4230 f 4231 4232 4233 f 4234 4235 4236 f 4237 4238 4239 f 4240 4241 4242 f 4243 4244 4245 f 4246 4247 4248 f 4249 4250 4251 f 4252 4253 4254 f 4255 4256 4257 f 4258 4259 4260 f 4261 4262 4263 f 4264 4265 4266 f 4267 4268 4269 f 4270 4271 4272 f 4273 4274 4275 f 4276 4277 4278 f 4279 4280 4281 f 4282 4283 4284 f 4285 4286 4287 f 4288 4289 4290 f 4291 4292 4293 f 4294 4295 4296 f 4297 4298 4299 f 4300 4301 4302 f 4303 4304 4305 f 4306 4307 4308 f 4309 4310 4311 f 4312 4313 4314 f 4315 4316 4317 f 4318 4319 4320 f 4321 4322 4323 f 4324 4325 4326 f 4327 4328 4329 f 4330 4331 4332 f 4333 4334 4335 f 4336 4337 4338 f 4339 4340 4341 f 4342 4343 4344 f 4345 4346 4347 f 4348 4349 4350 f 4351 4352 4353 f 4354 4355 4356 f 4357 4358 4359 f 4360 4361 4362 f 4363 4364 4365 f 4366 4367 4368 f 4369 4370 4371 f 4372 4373 4374 f 4375 4376 4377 f 4378 4379 4380 f 4381 4382 4383 f 4384 4385 4386 f 4387 4388 4389 f 4390 4391 4392 f 4393 4394 4395 f 4396 4397 4398 f 4399 4400 4401 f 4402 4403 4404 f 4405 4406 4407 f 4408 4409 4410 f 4411 4412 4413 f 4414 4415 4416 f 4417 4418 4419 f 4420 4421 4422 f 4423 4424 4425 f 4426 4427 4428 f 4429 4430 4431 f 4432 4433 4434 f 4435 4436 4437 f 4438 4439 4440 f 4441 4442 4443 f 4444 4445 4446 f 4447 4448 4449 f 4450 4451 4452 f 4453 4454 4455 f 4456 4457 4458 f 4459 4460 4461 f 4462 4463 4464 f 4465 4466 4467 f 4468 4469 4470 f 4471 4472 4473 f 4474 4475 4476 f 4477 4478 4479 f 4480 4481 4482 f 4483 4484 4485 f 4486 4487 4488 f 4489 4490 4491 f 4492 4493 4494 f 4495 4496 4497 f 4498 4499 4500 f 4501 4502 4503 f 4504 4505 4506 f 4507 4508 4509 f 4510 4511 4512 f 4513 4514 4515 f 4516 4517 4518 f 4519 4520 4521 f 4522 4523 4524 f 4525 4526 4527 f 4528 4529 4530 f 4531 4532 4533 f 4534 4535 4536 f 4537 4538 4539 f 4540 4541 4542 f 4543 4544 4545 f 4546 4547 4548 f 4549 4550 4551 f 4552 4553 4554 f 4555 4556 4557 f 4558 4559 4560 f 4561 4562 4563 f 4564 4565 4566 f 4567 4568 4569 f 4570 4571 4572 f 4573 4574 4575 f 4576 4577 4578 f 4579 4580 4581 f 4582 4583 4584 f 4585 4586 4587 f 4588 4589 4590 f 4591 4592 4593 f 4594 4595 4596 f 4597 4598 4599 f 4600 4601 4602 f 4603 4604 4605 f 4606 4607 4608 f 4609 4610 4611 f 4612 4613 4614 f 4615 4616 4617 f 4618 4619 4620 f 4621 4622 4623 f 4624 4625 4626 f 4627 4628 4629 f 4630 4631 4632 f 4633 4634 4635 f 4636 4637 4638 f 4639 4640 4641 f 4642 4643 4644 f 4645 4646 4647 f 4648 4649 4650 f 4651 4652 4653 f 4654 4655 4656 f 4657 4658 4659 f 4660 4661 4662 f 4663 4664 4665 f 4666 4667 4668 f 4669 4670 4671 f 4672 4673 4674 f 4675 4676 4677 f 4678 4679 4680 f 4681 4682 4683 f 4684 4685 4686 f 4687 4688 4689 f 4690 4691 4692 f 4693 4694 4695 f 4696 4697 4698 f 4699 4700 4701 f 4702 4703 4704 f 4705 4706 4707 f 4708 4709 4710 f 4711 4712 4713 f 4714 4715 4716 f 4717 4718 4719 f 4720 4721 4722 f 4723 4724 4725 f 4726 4727 4728 f 4729 4730 4731 f 4732 4733 4734 f 4735 4736 4737 f 4738 4739 4740 f 4741 4742 4743 f 4744 4745 4746 f 4747 4748 4749 f 4750 4751 4752 f 4753 4754 4755 f 4756 4757 4758 f 4759 4760 4761 f 4762 4763 4764 f 4765 4766 4767 f 4768 4769 4770 f 4771 4772 4773 f 4774 4775 4776 f 4777 4778 4779 f 4780 4781 4782 f 4783 4784 4785 f 4786 4787 4788 f 4789 4790 4791 f 4792 4793 4794 f 4795 4796 4797 f 4798 4799 4800 f 4801 4802 4803 f 4804 4805 4806 f 4807 4808 4809 f 4810 4811 4812 f 4813 4814 4815 f 4816 4817 4818 f 4819 4820 4821 f 4822 4823 4824 f 4825 4826 4827 f 4828 4829 4830 f 4831 4832 4833 f 4834 4835 4836 f 4837 4838 4839 f 4840 4841 4842 f 4843 4844 4845 f 4846 4847 4848 f 4849 4850 4851 f 4852 4853 4854 f 4855 4856 4857 f 4858 4859 4860 f 4861 4862 4863 f 4864 4865 4866 f 4867 4868 4869 f 4870 4871 4872 f 4873 4874 4875 f 4876 4877 4878 f 4879 4880 4881 f 4882 4883 4884 f 4885 4886 4887 f 4888 4889 4890 f 4891 4892 4893 f 4894 4895 4896 f 4897 4898 4899 f 4900 4901 4902 f 4903 4904 4905 f 4906 4907 4908 f 4909 4910 4911 f 4912 4913 4914 f 4915 4916 4917 f 4918 4919 4920 f 4921 4922 4923 f 4924 4925 4926 f 4927 4928 4929 f 4930 4931 4932 f 4933 4934 4935 f 4936 4937 4938 f 4939 4940 4941 f 4942 4943 4944 f 4945 4946 4947 f 4948 4949 4950 f 4951 4952 4953 f 4954 4955 4956 f 4957 4958 4959 f 4960 4961 4962 f 4963 4964 4965 f 4966 4967 4968 f 4969 4970 4971 f 4972 4973 4974 f 4975 4976 4977 f 4978 4979 4980 f 4981 4982 4983 f 4984 4985 4986 f 4987 4988 4989 f 4990 4991 4992 f 4993 4994 4995 f 4996 4997 4998 f 4999 5000 5001 f 5002 5003 5004 f 5005 5006 5007 f 5008 5009 5010 f 5011 5012 5013 f 5014 5015 5016 f 5017 5018 5019 f 5020 5021 5022 f 5023 5024 5025 f 5026 5027 5028 f 5029 5030 5031 f 5032 5033 5034 f 5035 5036 5037 f 5038 5039 5040 f 5041 5042 5043 f 5044 5045 5046 f 5047 5048 5049 f 5050 5051 5052 f 5053 5054 5055 f 5056 5057 5058 f 5059 5060 5061 f 5062 5063 5064 f 5065 5066 5067 f 5068 5069 5070 f 5071 5072 5073 f 5074 5075 5076 f 5077 5078 5079 f 5080 5081 5082 f 5083 5084 5085 f 5086 5087 5088 f 5089 5090 5091 f 5092 5093 5094 f 5095 5096 5097 f 5098 5099 5100 f 5101 5102 5103 f 5104 5105 5106 f 5107 5108 5109 f 5110 5111 5112 f 5113 5114 5115 f 5116 5117 5118 f 5119 5120 5121 f 5122 5123 5124 f 5125 5126 5127 f 5128 5129 5130 f 5131 5132 5133 f 5134 5135 5136 f 5137 5138 5139 f 5140 5141 5142 f 5143 5144 5145 f 5146 5147 5148 f 5149 5150 5151 f 5152 5153 5154 f 5155 5156 5157 f 5158 5159 5160 f 5161 5162 5163 f 5164 5165 5166 f 5167 5168 5169 f 5170 5171 5172 f 5173 5174 5175 f 5176 5177 5178 f 5179 5180 5181 f 5182 5183 5184 f 5185 5186 5187 f 5188 5189 5190 f 5191 5192 5193 f 5194 5195 5196 f 5197 5198 5199 f 5200 5201 5202 f 5203 5204 5205 f 5206 5207 5208 f 5209 5210 5211 f 5212 5213 5214 f 5215 5216 5217 f 5218 5219 5220 f 5221 5222 5223 f 5224 5225 5226 f 5227 5228 5229 f 5230 5231 5232 f 5233 5234 5235 f 5236 5237 5238 f 5239 5240 5241 f 5242 5243 5244 f 5245 5246 5247 f 5248 5249 5250 f 5251 5252 5253 f 5254 5255 5256 f 5257 5258 5259 f 5260 5261 5262 f 5263 5264 5265 f 5266 5267 5268 f 5269 5270 5271 f 5272 5273 5274 f 5275 5276 5277 f 5278 5279 5280 f 5281 5282 5283 f 5284 5285 5286 f 5287 5288 5289 f 5290 5291 5292 f 5293 5294 5295 f 5296 5297 5298 f 5299 5300 5301 f 5302 5303 5304 f 5305 5306 5307 f 5308 5309 5310 f 5311 5312 5313 f 5314 5315 5316 f 5317 5318 5319 f 5320 5321 5322 f 5323 5324 5325 f 5326 5327 5328 f 5329 5330 5331 f 5332 5333 5334 f 5335 5336 5337 f 5338 5339 5340 f 5341 5342 5343 f 5344 5345 5346 f 5347 5348 5349 f 5350 5351 5352 f 5353 5354 5355 f 5356 5357 5358 f 5359 5360 5361 f 5362 5363 5364 f 5365 5366 5367 f 5368 5369 5370 f 5371 5372 5373 f 5374 5375 5376 f 5377 5378 5379 f 5380 5381 5382 f 5383 5384 5385 f 5386 5387 5388 f 5389 5390 5391 f 5392 5393 5394 f 5395 5396 5397 f 5398 5399 5400 f 5401 5402 5403 f 5404 5405 5406 f 5407 5408 5409 f 5410 5411 5412 f 5413 5414 5415 f 5416 5417 5418 f 5419 5420 5421 f 5422 5423 5424 f 5425 5426 5427 f 5428 5429 5430 f 5431 5432 5433 f 5434 5435 5436 f 5437 5438 5439 f 5440 5441 5442 f 5443 5444 5445 f 5446 5447 5448 f 5449 5450 5451 f 5452 5453 5454 f 5455 5456 5457 f 5458 5459 5460 f 5461 5462 5463 f 5464 5465 5466 f 5467 5468 5469 f 5470 5471 5472 f 5473 5474 5475 f 5476 5477 5478 f 5479 5480 5481 f 5482 5483 5484 f 5485 5486 5487 f 5488 5489 5490 f 5491 5492 5493 f 5494 5495 5496 f 5497 5498 5499 f 5500 5501 5502 f 5503 5504 5505 f 5506 5507 5508 f 5509 5510 5511 f 5512 5513 5514 f 5515 5516 5517 f 5518 5519 5520 f 5521 5522 5523 f 5524 5525 5526 f 5527 5528 5529 f 5530 5531 5532 f 5533 5534 5535 f 5536 5537 5538 f 5539 5540 5541 f 5542 5543 5544 f 5545 5546 5547 f 5548 5549 5550 f 5551 5552 5553 f 5554 5555 5556 f 5557 5558 5559 f 5560 5561 5562 f 5563 5564 5565 f 5566 5567 5568 f 5569 5570 5571 f 5572 5573 5574 f 5575 5576 5577 f 5578 5579 5580 f 5581 5582 5583 f 5584 5585 5586 f 5587 5588 5589 f 5590 5591 5592 f 5593 5594 5595 f 5596 5597 5598 f 5599 5600 5601 f 5602 5603 5604 f 5605 5606 5607 f 5608 5609 5610 f 5611 5612 5613 f 5614 5615 5616 f 5617 5618 5619 f 5620 5621 5622 f 5623 5624 5625 f 5626 5627 5628 f 5629 5630 5631 f 5632 5633 5634 f 5635 5636 5637 f 5638 5639 5640 f 5641 5642 5643 f 5644 5645 5646 f 5647 5648 5649 f 5650 5651 5652 f 5653 5654 5655 f 5656 5657 5658 f 5659 5660 5661 f 5662 5663 5664 f 5665 5666 5667 f 5668 5669 5670 f 5671 5672 5673 f 5674 5675 5676 f 5677 5678 5679 f 5680 5681 5682 f 5683 5684 5685 f 5686 5687 5688 f 5689 5690 5691 f 5692 5693 5694 f 5695 5696 5697 f 5698 5699 5700 f 5701 5702 5703 f 5704 5705 5706 f 5707 5708 5709 f 5710 5711 5712 f 5713 5714 5715 f 5716 5717 5718 f 5719 5720 5721 f 5722 5723 5724 f 5725 5726 5727 f 5728 5729 5730 f 5731 5732 5733 f 5734 5735 5736 f 5737 5738 5739 f 5740 5741 5742 f 5743 5744 5745 f 5746 5747 5748 f 5749 5750 5751 f 5752 5753 5754 f 5755 5756 5757 f 5758 5759 5760 f 5761 5762 5763 f 5764 5765 5766 f 5767 5768 5769 f 5770 5771 5772 f 5773 5774 5775 f 5776 5777 5778 f 5779 5780 5781 f 5782 5783 5784 f 5785 5786 5787 f 5788 5789 5790 f 5791 5792 5793 f 5794 5795 5796 f 5797 5798 5799 f 5800 5801 5802 f 5803 5804 5805 f 5806 5807 5808 f 5809 5810 5811 f 5812 5813 5814 f 5815 5816 5817 f 5818 5819 5820 f 5821 5822 5823 f 5824 5825 5826 f 5827 5828 5829 f 5830 5831 5832 f 5833 5834 5835 f 5836 5837 5838 f 5839 5840 5841 f 5842 5843 5844 f 5845 5846 5847 f 5848 5849 5850 f 5851 5852 5853 f 5854 5855 5856 f 5857 5858 5859 f 5860 5861 5862 f 5863 5864 5865 f 5866 5867 5868 f 5869 5870 5871 f 5872 5873 5874 f 5875 5876 5877 f 5878 5879 5880 f 5881 5882 5883 f 5884 5885 5886 f 5887 5888 5889 f 5890 5891 5892 f 5893 5894 5895 f 5896 5897 5898 f 5899 5900 5901 f 5902 5903 5904 f 5905 5906 5907 f 5908 5909 5910 f 5911 5912 5913 f 5914 5915 5916 f 5917 5918 5919 f 5920 5921 5922 f 5923 5924 5925 f 5926 5927 5928 f 5929 5930 5931 f 5932 5933 5934 f 5935 5936 5937 f 5938 5939 5940 f 5941 5942 5943 f 5944 5945 5946 f 5947 5948 5949 f 5950 5951 5952 f 5953 5954 5955 f 5956 5957 5958 f 5959 5960 5961 f 5962 5963 5964 f 5965 5966 5967 f 5968 5969 5970 f 5971 5972 5973 f 5974 5975 5976 f 5977 5978 5979 f 5980 5981 5982 f 5983 5984 5985 f 5986 5987 5988 f 5989 5990 5991 f 5992 5993 5994 f 5995 5996 5997 f 5998 5999 6000 f 6001 6002 6003 f 6004 6005 6006 f 6007 6008 6009 f 6010 6011 6012 f 6013 6014 6015 f 6016 6017 6018 f 6019 6020 6021 f 6022 6023 6024 f 6025 6026 6027 f 6028 6029 6030 f 6031 6032 6033 f 6034 6035 6036 f 6037 6038 6039 f 6040 6041 6042 f 6043 6044 6045 f 6046 6047 6048 f 6049 6050 6051 f 6052 6053 6054 f 6055 6056 6057 f 6058 6059 6060 f 6061 6062 6063 f 6064 6065 6066 f 6067 6068 6069 f 6070 6071 6072 f 6073 6074 6075 f 6076 6077 6078 f 6079 6080 6081 f 6082 6083 6084 f 6085 6086 6087 f 6088 6089 6090 f 6091 6092 6093 f 6094 6095 6096 f 6097 6098 6099 f 6100 6101 6102 f 6103 6104 6105 f 6106 6107 6108 f 6109 6110 6111 f 6112 6113 6114 f 6115 6116 6117 f 6118 6119 6120 f 6121 6122 6123 f 6124 6125 6126 f 6127 6128 6129 f 6130 6131 6132 f 6133 6134 6135 f 6136 6137 6138 f 6139 6140 6141 f 6142 6143 6144 f 6145 6146 6147 f 6148 6149 6150 f 6151 6152 6153 f 6154 6155 6156 f 6157 6158 6159 f 6160 6161 6162 f 6163 6164 6165 f 6166 6167 6168 f 6169 6170 6171 f 6172 6173 6174 f 6175 6176 6177 f 6178 6179 6180 f 6181 6182 6183 f 6184 6185 6186 f 6187 6188 6189 f 6190 6191 6192 f 6193 6194 6195 f 6196 6197 6198 f 6199 6200 6201 f 6202 6203 6204 f 6205 6206 6207 f 6208 6209 6210 f 6211 6212 6213 f 6214 6215 6216 f 6217 6218 6219 f 6220 6221 6222 f 6223 6224 6225 f 6226 6227 6228 f 6229 6230 6231 f 6232 6233 6234 f 6235 6236 6237 f 6238 6239 6240 f 6241 6242 6243 f 6244 6245 6246 f 6247 6248 6249 f 6250 6251 6252 f 6253 6254 6255 f 6256 6257 6258 f 6259 6260 6261 f 6262 6263 6264 f 6265 6266 6267 f 6268 6269 6270 f 6271 6272 6273 f 6274 6275 6276 f 6277 6278 6279 f 6280 6281 6282 f 6283 6284 6285 f 6286 6287 6288 f 6289 6290 6291 f 6292 6293 6294 f 6295 6296 6297 f 6298 6299 6300 f 6301 6302 6303 f 6304 6305 6306 f 6307 6308 6309 f 6310 6311 6312 f 6313 6314 6315 f 6316 6317 6318 f 6319 6320 6321 f 6322 6323 6324 f 6325 6326 6327 f 6328 6329 6330 f 6331 6332 6333 f 6334 6335 6336 f 6337 6338 6339 f 6340 6341 6342 f 6343 6344 6345 f 6346 6347 6348 f 6349 6350 6351 f 6352 6353 6354 f 6355 6356 6357 f 6358 6359 6360 f 6361 6362 6363 f 6364 6365 6366 f 6367 6368 6369 f 6370 6371 6372 f 6373 6374 6375 f 6376 6377 6378 f 6379 6380 6381 f 6382 6383 6384 f 6385 6386 6387 f 6388 6389 6390 f 6391 6392 6393 f 6394 6395 6396 f 6397 6398 6399 f 6400 6401 6402 f 6403 6404 6405 f 6406 6407 6408 f 6409 6410 6411 f 6412 6413 6414 f 6415 6416 6417 f 6418 6419 6420 f 6421 6422 6423 f 6424 6425 6426 f 6427 6428 6429 f 6430 6431 6432 f 6433 6434 6435 f 6436 6437 6438 f 6439 6440 6441 f 6442 6443 6444 f 6445 6446 6447 f 6448 6449 6450 f 6451 6452 6453 f 6454 6455 6456 f 6457 6458 6459 f 6460 6461 6462 f 6463 6464 6465 f 6466 6467 6468 f 6469 6470 6471 f 6472 6473 6474 f 6475 6476 6477 f 6478 6479 6480 f 6481 6482 6483 f 6484 6485 6486 f 6487 6488 6489 f 6490 6491 6492 f 6493 6494 6495 f 6496 6497 6498 f 6499 6500 6501 f 6502 6503 6504 f 6505 6506 6507 f 6508 6509 6510 f 6511 6512 6513 f 6514 6515 6516 f 6517 6518 6519 f 6520 6521 6522 f 6523 6524 6525 f 6526 6527 6528 f 6529 6530 6531 f 6532 6533 6534 f 6535 6536 6537 f 6538 6539 6540 fcl-0.5.0/test/fcl_resources/manyframes/000077500000000000000000000000001274356571000202515ustar00rootroot00000000000000fcl-0.5.0/test/fcl_resources/manyframes/Model 1.pptx000066400000000000000000014177511274356571000223670ustar00rootroot00000000000000PK!Uvko [Content_Types].xml (̗N0EHC-j5ecK>$ؖ=-$DU "T3s` K# ,DQĞ:,(t"xv6?,ўr1‡Ƃq@"~#!F,/`(& w]P8 ΋2 k(fye܋RL3|:YꥐT'Kup(cHL項L-d:j!q NZvrt: 8Xݘv7XNޅ"u . ;$Th4PK!&  _rels/.rels (N0 HCn ]n0$J<=aJf?b)X%rZDbt{Hl y(u6$U\R1)hT@.w| ?%*;j@5[ n і1dGZ"E[b\NEyr 4Vӣ׻,i y$ ahDSB`"lrǎ#)Sx~g&'Y}PK!:N)1E ppt/slides/_rels/slide2.xml.relsj0 }v6F^F@Ċcr3A!~bа  }4.X o#. N1&,uGXTJ` c)I)G2& 32[GKm{ VLq4l@T7ΎzzS(WV(T,fKE˜Ńi|ɓqV`[C}PK!EP ppt/slides/_rels/slide1.xml.relsj0E}%;PJlJ UI?`Ʋ@veC. saǯ0)jeIGrzz9EҰqx|8ӌ3Fa5(f,S6c*kũ]UY3`0j(gۃ,5N &s  ųKֆjr=u˦?|h_879 PK!j9ppt/_rels/presentation.xml.rels (N0M|{)Nc1&01n>@h9Ny{F6F&r.W?m*eqT }l_nY$T! e [eWwhKXKsQͲSFtnwi(~y~X}RպaE% ]qm6m0 5aKSDV݉[=3|1Nߘ^H_؋O 1 :/&Z/L3%b# /6ђ;R0[OV';e6;E!5}azZXq6}Ɲ`$Ĉ€T'ZҞ{zEN`uC$y .GU/x\> = l.N5e7H]'ߡdH͍ UFl. ' ژo`;`%1A3zGuO+CgUˡ<͇|j"<KRY8r7FE~sP '0P!edk(bYSPN\*!X|$=R$8PשX_ C6\u<ڃ@%M?q.ۅs-Q.Y[ҜG'q(=bRT!ODbV5g 8LTka5k#jAdjzVR1;r3s¿Q{R ި^ЗQpKe4A?^/EA/o^ѓ8|(Gxy}c/NafW|z:*^hhz%=, ¥@!6^^q f .q3©pw4mI8C} ͭR}>'WFYpggheF+\DɻE~CPK!-,Xppt/slides/slide2.xmlYn8}_`A{kKd˨S٦(&A~-ѶZ(I~ϐKI/myIhCgg/^V)qɼ˙DJ)̿?^Ӧ=HffJz:4يWy.k.зb?r+vYrɠb[yyXKfNx44oVEݸV+`#v]9o+9/seO+r`vOah vėn&6]bȯ/ؔj/k:glzs[tҮN y-r{<%ls& c$r`n01b`{0J46{dGt`Z5-GHGR'sFSc>a'[T%sJ/ - ۱h)0+u6-_Bh1͏Kaƙϲ 1$(ʲ ,;v, nIU腫BHugŽ;;h9#ʕ.% 0$ju]K4አ9!%E~@OH\.d[kw 0+A1(Zs= M:!:஘q0 ޒZ /Dĕ>%JSi#獫$] CZλɉ_"Co ï[W|+s[ 7r9/ mw~teh&VS(8a-)N. 0CdbS<=*K;7#43RV>[;"^PD§a0zxQEOa )F#͢QyV1 ShMY>( a $91EɖF'JGқU>|*|>-Š<,Cs;P0U>Qz3%d5/?KvHءf&1O__Qs3?r,Iq H8;Ԭԙ3o~߿~;C@«r7ٚ mJEޛh {]PK!ђ7,ppt/slideLayouts/_rels/slideLayout9.xml.rels 0Dnz^DEd$dߛc0ovkœ5ԲA|v= 88OİoB#EYÐs)f YH8]H"S";UQi΀)NVC:Kv:gc"T(3rTzy.jY6knPK!ђ7,ppt/slideLayouts/_rels/slideLayout8.xml.rels 0Dnz^DEd$dߛc0ovkœ5ԲA|v= 88OİoB#EYÐs)f YH8]H"S";UQi΀)NVC:Kv:gc"T(3rTzy.jY6knPK!4͹,ppt/slideMasters/_rels/slideMaster1.xml.relsj AŘ5=ŽFwDڱ0H`s|}wmRV//O; %Er©x|8a'dvħ(ˡqn8Pj{a=_*mzt凨fiS3̀bIΒ9Kƀ\g]^{T3ZbWcqH麝NKÖ-is4n+6K2(Т+BUV!:lmb6!YSdۘmHox.$ǔC2xFgo PK!ђ7-ppt/slideLayouts/_rels/slideLayout11.xml.rels 0Dnz^DEd$dߛc0ovkœ5ԲA|v= 88OİoB#EYÐs)f YH8]H"S";UQi΀)NVC:Kv:gc"T(3rTzy.jY6knPK!n] "ppt/slideLayouts/slideLayout10.xmlVr0}LA>;67<@$L!}Wl=%WL~|IW2"%%3&yh52.|$c˞s;ܶ,3sD:E(ioJ!`2='U=O)ɱamEK/;`ԫ~8c_TE1W9ab4+E+H1އ!m1jqkX9}H=1#?'_NHUe9J>LV ZWazޒlI"ε(JZeUla^#J/y ,#3%]gΡp9s`A;d F|ѾgΜAtU[LJ?gUs-xW3s 7k#p>X'aPK!lxEy!ppt/slideLayouts/slideLayout9.xmlXےF}OUaJyBvz i@*iT[K=Z1+eW3ݧOp&d*γ(eAv|\UO!YHv)x Rr8E%ъ[ЊU#Vďx[aVᅮ`$)Xߕ|yDfsXu,ŢK!B5|јbV"Q)!"0R qG 6kMn'8m 9/jwǐb0M),}̃Orݯ  }F""u9B5v"Ҿ1=EBwos ^DŶ<ǂcc;kzbЅ<K%>%q08+- ѹC@,\В.1ԇw2  X'!AwT\FG7\׮&Kב,짴|g!H >b*׻'O&ٱa£D,E+r qi3PoC˚ uMWX'@Di @ 4鄈%2eDi3 ݞV ^:1)r]Kj R8Ld 5&oDM!BD>bs] wp&W[lKDz?D& [5CG [ ^G [tAC=KH i -׃|^W &r]O8(bZN)ƖJtO9k)Շ;&sU9LΞ-Ĥ'zHP=~Z\ZZ1[iF\q/;`ߒhy:KmL z4Ϸԙ̼N[56ia`)JF)@2XBt([ż 3v=(5N5rUyeeM"A@-V_aF^F@lND;nQhwQVw9ͺƮ2ԏʐt!<VMd&48")1$<,Q_QhkLl %JK2NhDQJքo>~fx S*=:YrncE^ ZEwibj؆6#};m~gH kv8‘?ءv;١0oNRW cӯ h 1˜l0pPHBKƨϥPK!W1M!ppt/slideLayouts/slideLayout1.xmlXn6}/ g c%u.(nQ`ΐ.jF3̐}h L,/7cҌ HZfW%'wW' j%eбN,I͇f%[W|rm; F^\ZWi[Rj'q*V_7j[HPC2DL8"OW<#%-`-ȊSE0FGQgV<3Э4EgK0nDZ;d?6@"$Ltt6.X[ u#{8n&9FM),}/ )+᥏ƌ-ѬR(o~(4둌0b[3:[^(r}4@v?mmqv]'r? *4\gm&~@[NbXi~^Arꔑ9/DVe$h# ReO.;̞? tag^U^C}BAfC^.EN)q{[BKI5 T:z>*2w qHAŃ̠Ր U G[8TP˜s[sAvFǝKg>BU!+Nv D = xG"?=Fpƪ,n0`׍ro2e8:ލ`D{E(;arQ?{8/O{*y4eۊgsx83 MίbSsI`U"@ =3z_c"Gt8t4G~쥽\ċw3U[VV% -Z0>.n, SQkEsK|_2baH3O-^xpt*>Kjz9;y< iNMS{zoNq!r K@wi~߾y\UB'Z?Ԏ x#h\ak;v`:PK!P/!ppt/slideMasters/slideMaster1.xmlZn6/wµkpuȞ]:v]<⪸(98/Jz?^ %K Ԅ!S45'6K:c@> b=GO[hnlJ7E*" tEHP/<,ǝIN͘O[ K$NOdYC_)ևn}(C/ڔO亃IJ{=Y֌?|i.y,"JjҚDR q3|sX/jdRVٞmH1ɡrX|Fv,Ku 6^|-isQsy+R dž+>p yQ!y4i|eI*-c $)I%+Œg+w8)2rZvרr[\Di?Fh)omuZn0UR$X%F?RHzJ ł)c<@yg%5{W:ܟRpR.>qo8|+w_{&7cM:b217`TaEcڮyMqoBjT2¦ɏ8ro4V"Mҽu$otv$LĨCi3`; :~6aR?ʚy^ov㏏޴7=N!!tɧ?t' a֮YkAm<?&HX]hDӾv9_'R #0\au2uέf*XD% ۔S)vޗ{m]~$JH1ffg9&+^Rr|Lu;kUUݩ׎2;vg3=Ww&&v宛RE_{"^td{T9YwQu:jCwqi42h-q2a[E*Ŕ-Sf[8nj5(]3ߦsk)Ϫ5zUVVȓoK7Lk䪳k_>}c8@LD? pÑ(FCzBuvD=rLWTiOdz'Qg^irHsËY/LQ0{Пsw~_8!Ub?YkTvJ9F #>m}khK2*5VmU~Ys01¼.#edJuq3X _Si8ao^0&7 1fʼS?|+hL_<ҷd3K7s,j|؟PK!# !ppt/slideLayouts/slideLayout4.xmlXr8ߙ}ڵ1@ޛ6-J@;k>NGEupȟ>|GǺ|.(Z.rVBLXBiPK!Hi[h!ppt/slideLayouts/slideLayout5.xmlYݒFOUށ"׮8[ꌹٝЎd&:T'9ݴ.xUF8*M%-ʘe=|gBsO4]+9"5-?tw$@l5.sn]siX?vTNe^;%qW&l􎅋f\4!/q^* [^h]:o &/q pӦ~ $2‰!KsR%?e>)(ELS!<, -Z?* {˟fEz{C m!ikEKW\ p{6?#趺X(;c)w&1OfnPK?se D{RHϵ*HU" _BLEj5:>oqt:21EH7dhk:$]0> Y >Y Ä*vg3F1>B" %pRYQ,z"#v1 W?p(~:&󧄄tΒ,. v"t\\ԥڮihҶ:1kԳ;'l0H", adᜁZL%e={UD_Y xTL"kA+閃NTOEjj"f[֎ !+RUΖմ}Cp#Z rUn6aùUz[Z 7X\_["WEliyʎ*Ns7 {B(^n#]+ rGQ1GؐezuGȄjV(^y+dVɘ0A~ 9-c;~CkBs 'NU'IYa m X%5P VŪ`ObCObNbNbA$ +DrۏAbմdLCEZB49Ҡ/^A?EsXqFlQycّQvM.:J&ә|QPXJDŨX<5,0&S]g~`^j=?jҴcΗC):y׶Rv0u^;':|܏͏:J;N "|:<*'x^4,uzY`8r\k`}pW㔎EA\G&6\^vA>b 7V[%206xw&1HGEdt/.b vhhljv0`o9aoZ~؞;pS%zuMK~ Ԫؔv]H$\'Px:rx@PK!.!ppt/slideLayouts/slideLayout6.xmlUn0?i`y)"_6> &NlOkmuR}=`hO΅Hrۥ`4 a1>R/'ߍPN#*$ci]D]rXK,OuIvܮnAr|yNHA6H;CM0$F9i&Xt4@ R Ft,q+iiy0C[v`09htݩ`u.NNs[5t}{ioRmIiRcMєT˪+_vDMcC҂Ud(ayBͮ?ѥ߆.pвgS?Ӂ~􇋙y܉ Ǯן={V;תO??<=j/R]/04wA&jospfvپPK!D!ppt/slideLayouts/slideLayout7.xmlUn0?i`y)$OFK`1vdӤ8}]n]'UZ$r=\dǤ*k13&:+fJo'DiZ)=0EgoLPV*)-nBUi*PgukYGq3 _n|Rc|M}eu-d4QZF20ϖA%6M0У3dyFTm Z2fVbA6f%mn%Ic u/iQ`.g }.B:_,5I`MrbBmoA1e ;"N5/3F.3]B~QUt3~4~ ˹y‰ 7?ά2vQ} fՎl{+ܚ#4W;<ghnC ~S>4_PK! !ppt/slideLayouts/slideLayout8.xmlX˒Fݧ*Х1P ff<34Ri5J+Im,!Ns[yM90Q&y64w=,̣$[ i7H)iQglhXi"(y@wZʀX"v0f)-e.R*Xu#A?wʻVuSdF^Y/IpLV$q*2NRm JQMϷQ0q 3pcgDdB Cab.CtUYgAj [Q `p=ZL4.Ez{C H?a V 0? [w,<sU!?$Otr/|h2鋘THU?U<4T1Ն#}-oܣؽoA02Y5q\r;ΣFtߐ8qK9;ip "8 " n 0 lZhx1\7x 48,e3hTN8@_$o'< ?%|dAۂe.e3j2c*h;C|pYe9 v3!suDPH'v.&T]4Qe]ۄRܯJ]աξjfe(zmVj`X:'Xm{49;a59vp[N$o+{ 5UTySUߨ* x<g[Ы޺~'=j اZkky }<cMj6T7LƾUf8A@aĔ/ 8D.fQ|֏خY?o70{GR*#"8%X?¡PeiNLD,t"[Mgt+==Țo`:+iwG:Zv~Ϸ|VqgY>#|}G;Z'#|]>:[&?WZdJZ%7:dV|8)D/<)Pg%<_Z߽tx8q\kܛQ#pU&)&`Okx25aN4.kk*`z@g$m#$bq.E\-P ͙yIh5;{?v;#u3:۞Od_%zuXomJ~7U"@hnrw8PK!ђ7,ppt/slideLayouts/_rels/slideLayout1.xml.rels 0Dnz^DEd$dߛc0ovkœ5ԲA|v= 88OİoB#EYÐs)f YH8]H"S";UQi΀)NVC:Kv:gc"T(3rTzy.jY6knPK !:ppt/media/image2.pngPNG  IHDR!J5*sRGB pHYs  ~IDATx^gIUrH8fT"94Yd$d I 3 *#I  #Ψ:A/zuUyv8:>{WZ>ܫN7ۥ 'uG/gX:ⱐY'%>)iu6j]Z}kmh`X`譱X7Fe[Cӳyu Z{5XdIT|llA7<7>p/\ܗ|cuSj|Nkww߽Ygvom-@]y][~Y+ծJ.;(Quv]AW}>vw9q1A57C4T}λSZ}MΫ <-'أ8?Σ_-uo~K?/~k[/3S{?f-g?=[Unx5׾j-(pK݇3u۵;3kSuR)WZ1csx3gw^RDJ$R]igsR}[L F /\SʃK8x+^`u:Ti:@uW܀:PY$"/YP ua+׹NC !:4&OiXE`_z] @ḭ;ɂVZ˗/;IKZcac}wFNd&'Ng flҐ@: r\GC]ݦԹuYs@re: t`dPϔXBkFnB1،o8$p]X6/NDsI2U N ;/uTn,M_gM蜴cmu\Ių܋]5)^2i2mR񶳕˿?ɓW'CC@ԌE`Qjڠ8'K>P:}ܑ"cmbnrdrp;'Yh\yPIM 0:4(rcmimP^]Ͼ ~CpQk09dw} .T5NʵGZ@;֪bQGb%fZZ#9as@d.)&ݎt*=%{Xŭ8"BGKYfF`F CZZ6= s87čQ1 zwevdiKqϥx,b(FnInR=KnEqbv2QݺAl}mQfpҷoi<i!@$[#;|o7cY $ 2a,e[>A<6b6l;ZN&,Xө<~d8O{IeuU6JVVd:wfc\g֋mMjwZt +v@i0n49R1pŲ3 ƅ-re,uElҴoJal}2?JE)-Uz:x"\'[20ţ|Q"R.7 %{wL>/c+bC2-y8/%ȯ=c0(8nW|$:| .yettٹC\a?f[Kd=uISe-XXX-cimJլ̲e;uEӍiutU o (=kx .Ꜽ* :Pg$2u+䒥teuwd9^ANwa:|ҹĎۢTwFY]P/~MKS%wj7Ru\VpS/Vn4h*T\۱uSuMC\NK)]S[ c js oF3?{C&#n'9{cj](9$ Dt uس ,Gǒǔ$nf*ʏN_EW-=M/׋E[v5(v+PvV!=dۻ*-Glwرbo+w̹(%_]񫰿cU/{E b/v JJ6!/G7:)ƌ29kK'a&9$X0Or/Ag ;K8;C_L i=TuP&ItxryӏGݓ#ْ JPxWÚprmiNSKVĞj8HFvVɳ;DP;կ^: &F4<žԢnE?/ oY:BkPx ṼN;`9b ,,mu u$!D.*DO&rbu%kwE1n@sES4\qsx"/xKq^u"L2ppdfJIXƬ ȥd%`Ld:\h:@;y| U(,qrioR*{9ʋئhYR(yţ~|"p].0: Iq[%|o%;ޓgn(AN6%q1g}oN 1~hPoUP lwK Xf,iWٲjz2PMѳa9Znxtg}Z ΨZhHWy~#MWSG&a,sŽ),i@j0AtoEX-M ~.)UKA`ZP]hN..lh#7' xڝ<݋ 2]R;s+fx\'o %G!+E)PPL^!VS& 20NBu@AkelDη\[9nL2aytU oI[V' Ďx[$<)3Cd'wNшs[ߝN&'w!9ZqwGcHnou=ct{9gm0auUgNd #u\]ձroٺ.hEo:S# [I`ˀv!gJ3^~9iL~o27m\7d5dffUR_@7A bdʴPF)a` h|/hIYhnUf$.C [?яaFM,ZӴ/Z3y~e22w}91ce/>t=-m嘨I'5,j^Ij:y' tw@[o"g=Ӣ?əU9Hs5u ̪Ғgayە$KjfNbi;9QwK=7z` G(tI]I:@Ն!{4 Dη$釼fc'Lz؈^:im0˫ٹC~iD8w/|^P^簶t, lzA;|.@:1=0dU[O/ )`'(VRJ/oх: VjȍWaX-QH}9Zq鰴CNOt]GΠw Z_Uȓit`ZMuoo.'>-K:YU@!pH: g䐒MK BGf*:vCmP;vң܁uAQEy:簰*UR{1ЁB4{ Paɻ[.ЋLX<4}PJ*Ky~Ps} yg+uG;Agu-i7LZ~cu{jee!wuE3ȉr|d:uuqBf9#r>'kFnę\`ˀ{ZVnz}Cvpprȁ>YBB760ƝTa oWlcX8[8I`Rv 9~+i9އ%sb/X.VM0ĹZ=mYƱ,&~+%9uqup.&(_-b*/)r[LwF)S [pK񶺔.1Xț~^g(:]˺1SΩ+VZ":}H}Mdr/.) U<xK 1NίE c.ș`5ucmDJ`YZwT1,Vgu}.uN*m&y;M> WM7CBAKs }i*\˖}٩Z}+e܃ M뇂˕uhX톻bxH]J|pvv :;c縑 XbWF8^V*=2YHe,ǒ0 %v1x+VuL(\RË"EƎ)Ux3fH $(&H+^9EJFԼ;Y2##8'I4[y携K\@Ҋtj/0C*q{ktZl|H:5Q@"WPVF'XNˍB[QKz˽\;u n[\MW\plwVX9)LFڜC&\yBRRX-;M$;;vt}6Z(PJTi5Z畯|{ֻNzί tbȇe|:mvN+G+{S'@Y+(u9[N.ԡήԕI+C.+u*Mn[_u"PtT-0@|fOug[5ISZbM޶ݸ@]0C}}sp}j8EԹ$L4ך*H´gb<tH\괫 ̽:Ng`/ 0K[LgL7:ˁ7 qMU[nOv6,Qa0<69Vv쩘)8Ik:o=9pDl౜oemѮ/Ӟ{\Αc^#uoߛ/rNU7ܵ8e]kUeQ M;33=Ks@OJ*qsǐ)c|=4&K@܍+ %-)'ܝ.$E[:p +Mf`s/<D7/;gD`;*e-p.7]XpkJ+Wl%1qoGΧ5u,.-4L m ҅w[:b0X^{ː* ȂItCf43k1\'7I~:]w۝m^=]yg.j 2VM?,Jqeb:u2ApX96^P#bݾt8o>A{cuwFkbtm7yYpug5KW_:k݀V,^32pܩC 3!Lu7fr*qKcɋ~ƛ[l=Tp@+o 2L4r:^)Pof^'"./yמs/g3RAZ]zXOwEq, ۥjE,{jz}[jUZP۸Yev@B[X'ƻm;Ѯ1{6!F4]-Ϋ9ğ5䋴) :h㕃7ޘ\a˽q]:Œ9rg%x וwqjᅓ/ȩI0-n+D;*wrtXApxpO$_Eܔp NX HV\۱uS{u@o-7Ovر]o6"_ il3-{c/an+Pcgeӱ.' &fxp/ ӱD]޺'J\r#bZӵ:o /:X7܍4Ws]P$訸;Dp~*No;q{;嘗r$7Cvu:bߠʶR"n2bBh1YKg^>(Lr]l).IyuIc7 2QӻKG^L[9uLtad$# 3wP0?$[|_׃b *\:u9hI\hJXJ%9U:u鰠Gq\ V9~ʥz;O_ȴwJ^+HY(])}ߟb[ :O^*vu) 6욼.rYPSIqܽ{K7Mo%b{^!i |.5U۪@XHuo7*[o?,)3ҋ.5v[+)|xv[m~s`_ǻ}ߤ~n7Kj QTpN/,PUNxOUk-Ђj{EXa$ /ȋboI1-yɋ|_e`VggJIXFU,{[N JE"1~o͐`+ 7KA_ _prr`~ .i*ï=[y:K@%x$Oיi\)p+r%|o%CѓW-09ڔ\> H&* \PF)P\pLs2ǤJnө5ʜN^򧦩v2p 5!:{GXcJD;.k]GgiL ~ˢW vBABĶ"GpE7w!^rp}h!R];CQY|vOP? (;~9^MSH˽a_}5Q A.7ICa4rͯV߂'6:JHPrr1%yN]n@k T9uPdM]&-*oz))~iLԻ_r))-R  x(<@/CGᴘ:ϩ˅K~Y 8 Ck9u&Wnq ^{'\k.)肜v gXeHkr)G%? JmDJc+`YZwT1,Vgu}.uN*m&y;M> 07j>kq1cłWVpCs6BUGU\J!hQ z1S=9kM{:ºÛDc%xGՁguT: VIpk[@zA"8udK(J6\b'W;{Jv`GU6؉yx/f9^<@ @Z\`k9P;* ;Bsd]srR:;Fy!{0;u\CkvمJ.$ -7YU|p}^|;aVT.0v3\[0;{1"й]ujg]7nEt:n1l7u֬Pxt*TN.r/ v?910ZY^zݗsSe+o'Lٗչ٘I/8D[IJ&:|]La' u}BgC[~@[&r)X"V;HG-rCPl7wP/a)|L܃5vܰOՠNo7ogN/ ť#eNTGaYANR+u/u pd>R7&uNY}/"$c ;1B^Xͅ{ ~"aOF A~0袀+rpV ]Lp8i':q b]b9urc{@n+ QFEz؜->вr8)}Mxv;eF^a:?#`>A9nc,fZN›%*ॻKKhܩ:u(sS_u<|1s>#XDB ( btr!2˗=&F`/ur9- w2tBre[XDW[f,o,pjޅBjTOw*V rupꖕu\SuddTwLa5闬Hd^H,C Zg.D :?rNklzZlKj:3?'?&ٓ03o~^kQ%ePGp s/wg dw׾\Zo|n'pBr@gCn վ-/=XՈȚo!#Xw9'{:Su {З!6a\rgxZ.hHgD,`纥ѥh=ky u(o: ue纁Pv[.:XppoM\(FdҾF`M\wl:B\s]?<M/þr8ǒ՛xr]cc=Ĉa_1aB-=lՊ#g闺*$ةZ{,4@עߌc1㬓fn1ٕ|+wai=]:?c~{br/6}!'"HxLNpG~ZbVdB-QS-iW+=:g Xn }7mpZ ;ddnw2pvmg2LS C]8q^3W:uc1!ora@nSO>4$_ԅ,m&-s}O5&oIW᪶5@TxuR΢]-ߪg!7FvD֣߅At4຅z3۹r\rjJtlxr]^)2$ ;DG)F~+z],fBqC,%[QsF$Nc/Tl: .b.}E[݅f^( d~o`M*) H>INc#o܋: q:ltY=uC2M+ǜYe׈H,VHsgGtCPs3bAHi4>؁@Mƻej==c;rpo^'MDZl/VDn}Glqf`o-Rh"~3zp$N*\KMA{Nni=vyFk1(۽9[fMSgM7Bq(ŪE7u鱭0}Z}\r1&bC/$u%!AR(״;]d5֡Apr/x{D]V'ű@9H_3PTopi> =uBv'O뙙;b]O-uhq;2:eItB;cOqq8)h|w=":Bws髵g`wiv-iwuɉuM{l}S@Sb]áD9krrIAs]S$wm]r/J ,evcR Fվ]h¹悜,_ךo<P'\/#mlwQ9eH_|F,pX Z+;{ܪwnnQCr lb Its#\r=2$5kr~u:-Xy$FwcB~M#iю$j kYt[*uoMn\?ʢэV+u>||mYj~@\T= N1;~9~./ݵa_c4^{Xt;H GMo}3Lx; :Zx}ܴb:ź\;_8Z?Vv#pNnTDc#Y~c[w)\{QF&O9Iۋn:Yxߟ1U:'Ot㊓sX'B{ Xe{g/Ѕ^夏:_A+mVBˍ4fr&\}W܅u[d:SDR$+]:X?\'4# !Q;s3[ lL(.ܤze-u-e U4[~񪅠@ץJ5eJ W^ 1hvCt_PnOl:/{ëuϒP߶1uq=FPq3|Yi~T]W}lSW;T^TOКw. R799|MK~rF3wn>AMD/j%{_\fcpQpѡ{8VQ_ؠ{gTܵ;^efQ=q,XgIsjjzX-Pmn}[g k S77rרVSĪiDg=8Wq*=;8|?!,1 c:'׭uh\g:T!@]ѡE uD19vbL%Mt0:7:}4%o/y7pq/t<.cye?d\,auᦊUZ6Z`D3~#nyZ8+?vݮvOֺ] EE*u܂EKVlN\xnCu٪ԡ d:lRPuO˧@T+unsGt@`*z'wmHvr'-z=LۣΛ:c<7Fkhɱ1\P;28C4Q2l79OCK@$h'Pm4̻7됁?ߎ$[~&=HpOoxKLR_k%1UzՙiRunz$: `pB}~Β{ ˱x;T0{u=M $aH,n i:X`@,[ή @(kZ%2fE{::Bp͟m_Y:GXV& U:r+#νMu'N}be:OL12u^XO9mPP||=on!8Ls T{ڋuUOT" ={jKnS[oX[ۻ[ F9m47]Q7!ӀSk[vr,n%SgPuW5xb@.It< :]1oc0qZ`9^ul ٘\\ZazvT̝/uTޚ:9]}]\T2*qlDdKѽ;QnO;R.ZP8ς̴W`vnyrHrZ-ף,J`Bw9`6-N1RM:7U4k $hg)!N\I:.d-y[y@st($̥Lgq e:maDegf*x9(i۩e!M/Ah @rA t/ōۭ #igM&$\Hvq[ -fQs/ٜKV} |m)I[Ks]Ltu;t,<*"f `Κe\]:M#*ep\`*4pSz%uFzO:>x\\P7Cd\4/ fHv{p]X㶂XAv],ӭu9klv0s1M8TSWnc[M w,:5 wH+M4LձhmQ~^ t$'9;J#FSyrc]Ht4u"YCTL;^sƮ vInsxb\VN*U 'u9YGkuShV߅s8 Dg=]#9Z݌:~]1?3N]?\fvN`0;^ӺJXtJv\yXn"֭{9Vc@'aV5N t\͊p vFn P?-omرDX[mK~fA[.VI ދ;W{f9[ 9~v <9$v#"Iyvƒb]U:K;b+tK@3p[M3;8[#@$Z-6R`6c^҄0%x÷;w^2^0n?!F%ϹINZē.T~ ,nduKXo驸7`4pr\[:c]XW.)A$Yĺ1S*j3ņp]э:X[ \g@YKWđh|Anua^rdBr|zHּ3=][i;tu-D,:nkn`FZ'kgD@#\g? &n+hXb֗qg;d}kd:V;(I;AtlzT&L-uEt)4nlq_"P쬾;%ܡnsph.,1wI4d,u&[:rUź]]ພmQer]5DA펬aCBPVW: N" ɼN},&Q4'8w!:\׏vkr5;gyh..uP6W9+y\\'-hOFu8d:cCb[vnle2Γo8&3ۋQqۺ[V[Q]^Qn7E^7#٘kaͶ0.+(9<7A L^krcb蚸N\9y`zX, N4uKvnc'fÎw/諵Tlv׿ ~-VN*ڭK '8G]NX87萁Rt:%vIhȾ)enщ=.G}gW77׉Xg%E[tAe׭Ct[ẅz8!ͨex'M;c::ڭ0TFCFe]|Jv^WK[3N)Fޓ_g$v# ~t1׭It+sݢ&>/qA~Qe[1Y赖'[m{%n:;u fo@d Us/u[:@uXԧ٬)g\嬷Iҋ_DŻI]xu9vA\c7t]{ۏcFVKLz>:ΊѺ᭣̎Tl։qku5I.n j¼U?)L`s˔ut/T8'D&r\Wu4?? Š/?5WEc%-'=̓M/8./\\^ ,0pxsv\7pLS 1]TӶܭljt{P@7Mثl}r*Ja]>v&".;hAt %;z9= unN7$9jt[yhS0]OLSݾǔf}8Z:Jv@tu;+uN7S8^lTSawE1iiZϬ>ZeĦ64Tn0~1<6Y^ԡUNG[e8>eϵ3u|7~n])Nrqk"n@3,tl{6+γ(uwAkMu!,U \'Tp\~>%2%(G'V33Yw_pj6[-ugV(yfsFvvpV"0>%Z5v} ՛nm)b`K_ xK8uhDF}Ӡy{xoQJѡIhtU3Gnh= {UC].#q `@tluMD@D ݮB  3L(3gIƹn?MVѷ=BqAI2U:3Dt`Lwwo,IwCnhP:ܳE`/4{7,HЖN-U]y]՟ahmE9ɴ]]5V$j1[|M Y|UA*B+&aS.[W։L`e0Y= PXW-ϊsn<?Ah*c33oɋ0exU *Zk3sN;=ѥ, 1zK}܌ҖV,v|g9 .XBtzDvMkP'$܋ OZS.}SVrYu=55]Mwat8"$|:?MFt#7,]h .W iZ%:mf&mg\G 9 4jtߦqX ։CS/\$o;oF5v<\Wn({H4XXr@S:!:<.$H;cZeVL7ȶuD tŕɭ5%N;{g{}%o] Hۚ'vA;?S fn.usxu5ܾ):Gs].)KQk_MtRwQ= P~icsJΜb]% u土Nأ;jx 6ws\֟'LHNٲ5~HjN)؇&k%: 2x$WӵuE wIF)h(1)ҔrLb5벉t&:l 1*p[O,@MS3;}{b7y8\RX# ;5D[o~cgv~s/C\V˵hps9k1%֗~X7$"]Rv:"DW5hMlB\G Fd [43uRfӍW(3{xahibθ^_FgQkQO:֌tIWt7iܡ)a` b]`\nMri%D$9nLbc[KukJvz pn5ƭ2KLhlƎOӣ0RGE["֙dQvʽ&I9D'VdK@WcrE۝˪XW]P49d+ѕe\S.uhe+\3p/|鷍_ʌEn]vu @S^ LM\&C;7\J/^?Va| ( \bs`g2,p#2Kvf[b D(֙YguKb:zqMs:8;jGKH(K3Ok,,]vXS^QmlŚr/iVpDOŅ{M`NwzL \ su W쌧wIri$ub*rmwɅd:uU.؉@:Ğgi𪫌VCmZCme@dOrɥբqFEvZs/nD;4cnueR״!m&:r+r [.3ȷLnr]ug@Nou0S=qon{{Wx!Cݎ _L''~E< rd9&[J%PcauAE;e:%ֹ-[:S" xuitKݚAs=2pCCù΂sAr]g*&Xsg[3Gpn-rUσ=?uws\l2tS*AӭRT?;i79YN\@tUvR)rH#IcE@XIb]ruud:quKmPXA\ׄsdqp];,~ Ž!,aIqr[錙GI.rd:\Qu뒳Br kVY>>Xʴ&^:R~.po9s/I8-v {Dqw]1ّGG|NzYjm'Bz]$33].w:iXЉ tw}vZu|!RK W%EMt+$^,Iܒ\gB[b:R1qN:>i-u:xȞ\vptU-Ae8Sӝ՟M}-3~Bm5\(lteվN.XYd_ 7u&r'`AtbۖLY8'p2Kl/wr]5DaBݢw]0?\g~hybeSյM%i:\PWU-.rsxTɱ2ϫ$Qc\-ZGC翮ꏟ oOF+NFJ+ѵڏ\jݨS}Yc0hrwNE0nB3nCwȨv K٦Ѻӥ7ec9?i!ew]D;T,5tvcu$5PI%ĺKw޳q!2nx_l(uի),rؔ{5]r4Y$TT\ O\$;wo di"p7 '~|AvW`!us_פ# uy7l17v͸X\wvw$}⏱dov[/2=~FB@5p˪<`n#bHCoc{g\,%;S簲 04 7cRlKGOԺ5α[ V>5uOy*簛%q#=rIM6ޟ~U~k I7>Uw4}M fiW}6W onkfjv: ۩jF٩6+P֛Edi"~yŁAshrң\ qW\GOgW1YsL0uc+I[4сt?&ԡkvs@V#L˱MNy{J݃n؜A#xXvNwֈpq|4Aq݄:ߔ+~<B_h+qל j7(&d=:P@U#N*'a'^ƾ5^4mʺNq=mα (uƋU- ˆPB^)J.@yu3s8Vv3uԐ!Fvs:ǝb2d{ג%շNWsY=5oG}{|e>LdN(hOw^Xr/˖ʺ":_D]RLg`8ڱ 솫|?{ QV.8@fqQf}\Y0"Pg!;Q}K=61G[7b_;2`ܮe~bpN]nFFw;(?eL EKi I!:lbϽF4({=uwOI:oE; rrKu+]k}'6k{vp]~ p.8n+cB`m!?[Ͷ( 8S1rj%lnUAk+ItZ *FwV(vlOi^Im]  ȉd(ڭ9>gɃNC;2vNX1bM_lأO":%&sntl9u4ױ*ݵ.wG3.k"V>n[A{ĭj:{tIdex\;@!ecNkZG`ס.G}^v Pegc\bql/wK[Gwhp&cKC;d-쁉;wZזCvIaǂ`ǔ‚:i_+]{鋃^/Lrz4[-3g_دg<Y"Pu4`ƾȝ\۞3hAZ~H`haQ}UF!oL#A="RѲ*ntwq\*b](,P*&p~/ [ZK,2Y&,(oުJKS7C}(O`ɦM/g9F$?hO_kLItd9\͞^pBtM3*;*9TpoۢW88 28`OŴT!-v T~B;"^?Neqc8 oQ驻cLkAtzP UWOl\brņ;3j[[t[qJtJ,#ZU72'5]X:U=yoĺ鸄zkuMźnf\Jg Zd9iqݢgMh^,gɳE`oBxvjPJxZ|\%nqZ'I0E>_L'\rpst.&cE8vrONՃg$nuegDgO#drq]z]68̜,u=\g!;ew(UˁPK~ KOW>*G@'YUui.$:)$νKJ2.t9kA'vthb~7Ă{]҇XW^PwxB*,trҩ+Lo~]LS=;vG(Rrt}qHB(ɮ 1.H1AwV? h!n3Ff)DbI.^Vč:n2 njwϸnG Ź;$:c@tΓMGb6IsMDKl,_ ꬘fFE`=j5`N;%Uuz~`ġ: E?<$אb{̮ѩ`Y]U˥YM[N%k H,U׵6Q.OC ],^MS.[[q'/[=24K9ݣES1v TrqNRs_pGu׬2#p"P0B8oGIU`n8CPfcw(&&V9ĽuZ)P/@<'dⳄLWN4+nu!Z.hI\P [Y 9d!Dюٵ.OES1uMhsQʢn}g}:mpy&AA] nF7F]9=*'{~ a2DZOzS+4ȱ&VI e:Kegt@Btoj:Xs/E,\7v];I.Pn(Ǧй,I+Hv99)rJv'4xcqݡ)sCg\Q4,b)p^E@N&gBnO{bA;Gs(;n. :ceRSe\^7]Rrs]l!*qA,C_Bt.P}LO~Éx%h*B*Hv>9ҝi,s#P7}˿{b($Xt!F:w9[aQAH΄LW_:\` iSpZ,U7\$ڝ|ǼMGe\ʺ~"#:cZ9!:!97Mh*f*Q\ڬr"Pu;r;8d9r;˜),] n -XUNgݠLgg;;NVIuFv[/ԂL :2 өIC8Mse\PoB~i:_.s]|Hnn '@--ٍM$zOᅎ֟Lf_a=7 _s֌~En">`Sw|Ob#=]Y @(.VZ Z~luȇ#4kS߼[. { :ʺB4|T|A^"ڹ:-@t>n8[3 b&A,p:>@]MUS :߲vڗ`|ow(vz@.RKl`-gO1i]fLB\Wͽgzn,qm,4$tOk[D$ :F͘i!0#83ީ`X :TiDNX[΢U%cϮnxRp'V?tIF{<./wҝe:X%)Keu:qI te?5D ^&׹'x8aRO8j(o%,u!Zw/g!ٜ ˍnTŒRd, yYƷ'toDj-Cw1L= DۜK{+%+B빅v2ݝlo|(\c-8n.Ψ4ڿws|[T|(E`q9aTwc-:r|eBշGcv`ɪAu2Pl]-dGՏF<8ܨN@k: &鮕`0 uIք$% M3 adw.,DA?p]ޒ]u@߭@h\ׄsX:N*9u9.u؉nB=F00*K1KE0({XD&fX窓DGu> ErMb>GwK. 1k>nTD/t1p]X1:7:L'"%e: %n~ՍVL x#7>n9 h:ssCnQ광ڹ u,PWӽO~ {GN57щvIM]+>gfRP17NV ճG^,-iu{ƥPWG~StY|#q>ZX?@ir`MjȠˈ대^W5 ^pEp];@AT}F,ql-LHG2pF'%UsH'M,V h7ך{ɔK!;~Evq$aB?Kqrk 5 *-vskL_1!SOz~wȻ|:{1RW%/rl~qdcVMX0`L|fÈ*uQ_gur]Q=i -7(8#%o^e[gi?}@A-:S&:K(&:-U2*xCMz(N_(-$r=(\6&:\1;~ꪷVCn s1ò)cC.8t:ciHqg*{ijǭCNpe.ICѕ'^p5!SW 3W%tg-|}\V>:n8K>h]GMDpIPI>kvZ\eM Rf!-Ikw5 ,<#ÇpltևbZzW #şj\Z!ejQ8/y.9/)_(n4079i"QDJDž"֕8 4Ge.BwY6}猹$U27UN "\X옄7>YhtC: pX'vY5 to1-~ZV{O].(ٍ!>Kw=ti?,7U"$ *~4E3\'QҀ(qAҚr$ɂ:떖D#'Pr٘`9^|X\|NY0KZ4&EX@zo8o{Wֲ,g?bnTGX cef'2t9MtآN?9HB|\ʉx,9 tB720-/{ѻL gcjz[.:MU]u]R4!,r5n+skHW7DEtɉTv@W-]МuwEitǷ[0b9~t]r| Q֗Ko1gNҀs^*r/:KqILӜ#b6YƑ-k֑\qd|z8< 瞖!EAroՀE2t"R:ZIzX%]Fc:F e!_{+̟ĹՈΈs>.0:12VMZ0`Ms:‚:˞(=n{[T-0e:yc6f媃u+HvDQ[,P(3u[Ԙo ]fuf;bb eW%]ັ1]Ñw39!uAx8ZG 鐷),i.jb :2*r5K,4d,oKv$%UN~8vPWE.DCT[jOvntGu>tzY=K`,=a]il K;:- :ws]ұu. :dUS.Eu>![\dGD;hd9Vdjcu&U>y,y8Pa~X 7;)݁jܭ.tr<:=)XԍVh"Vu1(ch,klMZ_8$-t+\#:FrNf{9JA^_+$5M~kj\X('UF;fcp=̓xQ6sL9:f \7~Ծ!X(}ib骵!:GzfՈ"Wk|N q9FQӵvPmJts2]k \\P'b]LDtvst6):!4cQW^u%\bc1rsn<3v.lO;3$\7pԛ-X`DȐK{k4|3>dZh!l6G{ŕՉvI[LWZ[, tɽH´%'ӱ'ʢcZэ=.pDVN47A#Ƀ\7nTc u1 mͷ<[;pQ#*\Uk h+g4IZCo, KR6Nξu\`G⏅đ)^s$9["2>wfN[T+0rxu~~ut>d(1-$Dsx:w>ngNpQ bxFּBIwgayQ?1u!P͂^4gUp.&:2Nt=8g`|K](6pq]0n_i HH8oe`Y]gP<]'b<s-:8?*KI t܅/^D`B][} t%^;fqD ZOFZɧ*xTC['A,\^ur:[tW%D72]Seu!\ש#N3κX,֕j>K9R\5r]ڹe:֔K{@J1a9✬3z>1Ŏs&K+IS 3l.R&E*,o;a`if vQH}Ɂ٘|K(e\՟m tuIPT ':e[/`9۠QDrtXt%;_bL_)_QJS6<`_Ӂ#0(6ڗoVwZ9ON`=wSģ4~QCѥbTj߈vE@󳐖9ps˜L$5zXFr]XQX:w{e M2]@qz{OF+99.&xr!ӹ-W 8,c>I~'8Nkjg=.-c0|!:-hTPkNpWv D{) +uHT e(=oӈvNd3* QTs/;﨎݀j>[\g{dSeX'> GuwuMv thk9pf>̓q=jꦱ\ťzfËA7 3|<=]+2 f,hjr M>à n ckm]eZgɷtݓ܅Cxާ$\x;+t[X@r:D$׭vb]Rr g/(u/ KS d~;wa9ND%ΝoY\hW ,p"p j:| 'P,փ$jOȜ=8$ѿ*p^UQq8ZßICcP?Ml|[#Lcbd9`[SmXz䭏k4ߠt ~2ݖeA]$9bƞ*ubV9[>6Jt6̥,<..AtR{vuwr竞ꏟ֤Ա=~~t7s TY~=g6;[ D v@-lf:,vP~(KjۼX)`Coz$Z23!Ş&uH THyĭ6߄[tk,PD nP>Vr$UFs{b`#%9G;Dxi {Fg4ɌүhO:vSq$YfTվ1D&9t'P׉stP%u$- uD'P$:T T6z-HLNN›?ZUX'I[z uFC] p:# P׺%A&?!E`B]v4ևiKxˠ@9 N&Z^hg5wv+L&oAu{=%:wPh=ն0d)>oc˭ݢXpN\-s]ANHoD@M?ݕoGG1vANP'=\͜ɯ+@*&.VP :.P]u & իP']#ŋb+gZ&?>8_6_/Jw蒣3_<&='NCn{<ػX-,jJ=aweՇT Bnj!FCV-`qr0:dEiOsÉmfn$N'rYFpO@E_2>ԐPh4`v TLvM.Kȷĥ+ ^M5qæT’9^9SBtb:]#&9\@8^ɺ9\X5ǫɾ0XpN.Y &YrFXE`:ɮI.KK?jjVԱQ][!b&$+5 tFF?m&2^.XD7PKBAcyX7DWWE :weLt*>Ht24@ F0)i.9 b_iP2ĺ@3(uKs:b/PgI$cmrDgWFuZj8$ƽ1}wޖZ2}!7h⤥:V˅`\NVpN\q]/L"kPBt~N/sY32,޵@+o%je,Y:8:;BNX7U/sJ"c-#=%Wz}>+lǯmOv+?^f9iꤢ^:_^D`IqçZgVi7bSwZz =7:Ъ刮0 iЇ‡Z.jE~ZOuv ЉFt`˱o2j]r]`q]/5WٕV.n$:T|N@s^jQxza5^~sEttgr%W@wڠa9AcIt@b:'odf? ƸboEJåQUktF9og8 mP`v&q%q/Z5Α.5{-L/Svt$:K_uD;K2cə&C4 וNK q䚺+*>VLgӸ?TP&ri4չ9J ѥp.Yxའ(тC@ .cHgCC}هx{/%*Hsb(Ob;9pKBdVqԆ)8W[4I xr]bH1nDn6q9\gʖ\ź2!2?GsՎ/AtM^vDF/W `<*:X6t7-8i0"p P}?~1\' i.NU'[ -_t(J{ߨnRXpgcZpN<u"Us+Ke\uc\ZÁ*C$;ر^j?m $e艹6Jv\]!߲_qaY6g XQNxSkY8G?(@iNSt{dCYD]fhU`,<^0 c6t]ptj!G µUe:L֗mQ[_SuYݓ +0[zU|*\]n}+ҟovsq,W@?ҀHQ|? q[^o>%elV`AݳJAj W?t7%b2'.gׅdxkZz:i|SpVJopސ-1{>[6hRVGˆ匁UCCe tFCC2]%bj5/yA2+&7) Z[Z帇j$4(tHLC.'â4itiur\e<@9)EP\: d3n"o52Á:U u?O~._w}ÎT%V޸'ݳt91i i`,`qR\H}sґVce5:ۉvP\g'j|PG;SPg!:~si':رC 'u @uɣ^#I-7|ֆ͌ ;'?F5qnBq/"~>~_ Z^\#:4 = pFD% xVѡք,p"p'XGhbqŁsvךWu㯾nfՙwr[ E 5.)&S q~uo9膌 L̹ԙi$B@5$Lirv 8rrY<OfP~u7Gcԣ)5ppD?rrD-ݰ$[ڭ%KA VIt&aǪS1(ec+0wK'a[zKKt΢5\P4)uXrJ]JkIc^gtHEK&a4G#T9i2z6uu4eJ]K Ie{9@}ص2?qo:&n4S Ꜷܪi_UU{ }/%8 u3v\V30h"?;>UGզw>CT,K{ +iqLhD@QF D8++0s`rXW9#Ru^$5'Nt tCu>rduuuuK@]YAǽBDuz] uFcv#їW>cuLi"p8PX' y?p͋#G 2~u;|typƫP'qd u< )b7G/csh?zC W{X,AbC#kV;eo^u-ku.*)Os:8'NNNtt )唺NN:Z%Z6L`'ӱ_:s99D_Pe:jN&rPW]J(uMPΈsK+A{}u$:@]_Üϗ\!x\\Jb~0BHs9<&ѵ,8LuTu"%%|ǿ現}ʜ 9Zku@Xm-gd-3hSa-DuAG[4߲0en!?1 uyP is0s/{.k>B^;_l#vDik":F5J2v#ehsEã uOr u:R7-_qNXNM+-%-)Qu,N/AËx\cƓ\#΁Ÿ|}go^{0L۷ jT?[q&LrDíid ߲yKy}2FсXRfF\1m#:6 $:q1ht US.٠b uli9Ϡ꿑uZ@'DZΜ:,pYJTj8 87Y`Fࠔ: gyLf`$aa8!H}rN&ٓ tlI}}7W-O~ȫ \q_,xW9Etb-&8RTDWAF1n{`Ƙ $;-uwex xMidAn|nUmTU TPܐ\T#btU ;Թ=\'kf'\ѡI[(r 1!Yݑ\vpjwU9[@t|!PWN,@]jPg':x:\|:oQHve@]PvT|᣾:QZ:TNKs]flks=?zDgea~Z;@#bIru?tlL<٘ :6][T:7alH_nTcrA:`Kg*AP;.bc0^Uu=MuTHg@w?nt6;őP']rҜ$ u,$٭upub]ԵB>'2tIiNž#:)Pr\W `mѡ(iïJqٜ|%uEt$qX#CpLk[0Z#EuxPCpܥƺBwuBNЎv^99-FqM[b*ҊLۄ%xӝ#ZM652bhuWA]aXxP݁:$-r)޺.9}@`$IFKBP] β?JN++u*uFccPDtDWf9*ԡdYKB]莾\W&:T7BtBMۡN栮- Nb_pLN3Arb@bA?s?yYb^y"A1h]Kv`h[}ӍraWo[?6>&wPJګ –2uPn9>?r<t%|uvgur}gm{ًw W5M^` E+\@2I[Bwx%^zi2ԱEuUKIuDmf\oqPg!7^ ꂣ 2iFP:1'UCal)u'~5#2q%c/&0:?}?G\GBwrD2ӓx[~~OTuvƒba)ܹ;X5(#NM^72^8'.u98u/*= !:vHvF.]@T%; unbjPZy8Y+='ṝrh%:TB٘nA]U},@XE~*u@G;IK]ƍ.z,%q' 8 u'30%xb h\Wv9 Kb&Kh2=ܕ kIo7aSV/_4Ԧ]NFsHs9,Zq.ig$X bkB-{3s#O>>$VAQKNPP'h/{ZaTDnCXkR):ɽBܓpv{-uHvD;5'.P2 :RqS.)D3k9<8q+uڤXCJ:r5m20(Kn+mpNjD |Ki"&:(uAB, XeDus`9^Dg|>#PB-b (u^du$:]Nf<^`\'eʀ{`iA;i* Ɋ$F clN #f'i4:2\|֡np͓u䤒P$:Tw@hWMŇ>PPWP&Itx]C]r7BtXD,u$Is%g8PK I_:ECs:@p]*k1oN A]'M\:qZ&OSG) ̴&\',mϵ}z cVc:MthKC"H6P\uα6l:-du2# b]R'L]Y;ȷV:CFeϾB n =PghP' 1R?sUR.4:$1:ϊ3:uO[0</W֡:I Nu J4 PW)KvexL' ς[J҃ҏ&>n#ΑAӯgZ&D+ùPsÆr]Н2.s0]:6&:VX:Tm~n VD2M/bM :*C]NCER{𳯀}[v8PJ]a\ o,9~b6:_gB\&V]1"`^1ѱ˽Phg:@sW>79w~sQާ.)NN &stx8JB_u1Ա~ul.)ٕ.'ɼ*@QzNX7dT*K8 {Pk䷤3uxu+@S. u$:Tt@dӢ؁y :C44b&Jto$ѡC];ԑP2]D UNXC>Us)q.xLu=?ߚpDxAIoeu7u?uƥ[ Q0:c?}?d\]uZc$b-NO)_frRZ0cW37m/9_OfVȱ, ߲p6R`<:4:mx1 2ѡ]:^b:Z$; uM8G;FcX+C]hVϷ upu1ԱwF.: ~rDFuPJ]q,@]s(sxW:/DIt | #^ (WZ;#Η%Yb(xyނw Fx!SoqY&:XCNc+g]0@_N9/lyq,,D(\BrrpD^ePWSД +8 5t\Ů*c = dBI]1W;tJv|͸JnCcMDgS#It9`|I)7{[딄RGnp@t몧$.? Fo+!1Du l]3>Q# AXG\o+)Lï~52rF;uvYMw,+XG M\?$:|þ'D;7: vVOv hF$,g9M b,& *uVXl[\~:]`!9#iUgE^0; \g ~(NR.u$^&}2 q,'8Gnsh57A猑f|8P:lS.iXXN<.t\"{䋮MeA0j4Ϛ[Oc <󪌗:IŲIķ&A] u|q9s@]堮?d뚠.8/[0Pt$2cuInԡkvk:XPJt0%Pb@4,5X'/ ]nE8߷Wfcuk9bV:Tu(f:!:.hQ2*cu.(@3IKr]`P1c fʟ.7PѬ6trA2U)/& UtuĹXKNPl kVۭP*cB-sb5u>α]:g亜L>.u0-Gt:;ѡ@]nj:(uZ_$Iyݵnz/ :[  trn蒡/,cu#30cNC ި'aʩiTgݠ>FP<7l+ѓrc!|D?}PNJ[ؤdW2`LtlȨ١NR.e"i㋭\W:~~ュW:a:7q(@t@]?ICUc~pDJt CcŘrP';[>9C .O uxu2U7uKPV[b>្;+Va[`?S.OHkzq웞 ԑP%uP@g] p~:[05gG`B&;<d\r@hP_\ǯNmQDC-uC;#ԉBwx%27 \Ĕ4ѱzry/Y+ $~ns% u,Ow=\:Jã: 0UPGS=hW:71L~niCPWd P2UB],I 2׹e:ƓD[L{nOqt ;e__1BJB1ta%g1:+] <ۥwK-B=\-dW$iC]u5v=\FKr]}#P>A'Dq,[u uR\'3:JwCjs&C-,sONC1uJNCh u(P2%:$:e2 ĺReΨѓ׹8+үPz!PׄsDU?ܹ/PԕNR.aMx>fv$ =,{\Be]Н2됁 PrmA˝M # ;\c/σr=B^pÞE\\N?n_%:)sDWvٌ])f!&q5Yp9"ΘlZjJDAC=ikQe]"U:S.fV:]u+%^Ƿ A[ R^'WYJW#/N.`9xn!ŀ3^uFY.r&q= ?vuv>Krv7PN B].L,xrsIӧ m]Oʥ_)P%@aR\G5.\Kځ@W蚚Nn{ x 1Ճsp :K$$:76uLceEgKI[pt.Et |bugE#p_C<X ?x?X'\#'֡ 4uzßۼ\'hW;ױ'9+O8 xIOhr2$du=hGD~.7MtW:9@W]F+X.WxF w; ^! br2ost>Pkjt1AFg'o$Ht?x?p57г֌r8Pi|Il%ux]ωve:4A9ɱ].9DM :߬Mr؜%Sr/p]d] w.oݥ9߲,wsb],GN7=|h7|Rۊ^u?Y3-})p@n$LKdb՞f`:d:!LlJDdʥt:EDoNn\u3[(EmyZ @lvNJk4`E)b$ny*p.{x|%Lu}Swe]t> ɼMh@,7ubaԱn)(M\vFIMqUA~&޲l1InWxǔ(&ۥX[v):oV =N*{;\o :[87 toh\%(ɷ`Cw6Bu8wɺA/, }S.R*)R2]]J;T7Jaea*7J d:"f{`D_RyP'{h. X 6J-#i NpNB17u8WK.4uXV1Z;B1Oc\GC?^os2ѱ"5sݦ\ו(ӝh\o#4ϱz:ANV:)rݑ't/TJt~Kte+ ~ :&~[vk:sl][n Ե tFCsCLi: P'[gЄr] DFItz[d@p":+ (|WN4~gDB}$]s)N@: פ^CJ&:=!bUCh[%-h*ց8/hxLzunCs1Y~%XG#h,uBxGMV Wy͟o?t31p;rhG #^!I?ؐPZ(:#:Q``Au=2.GtF5ѡ Y9 Ź:tɔvwW1咏)I(Rsp]09q&^ :s:X'UcBw8ɠ<pX(ٹN\")! C [ԱQdg:Z.(NE.!<u}'?DX[BPGgr:uu+ Y.s{)6u>mC{d.n9d:z2`P.:;ѷ*6Gb]+U- y|nAn(uIt(23{c uVUuXMDݤX׍\ZζHU3r]'~5'ʝM~؏] Uv]+dNö%2(W|T=FF4%||횠s2r]P&:uIYɜɶPz#p]+αPY>52PuJ$PW]> u.[9J{]&Hf[?gqΗ[- ѱ$:_S4un{JCW:'BMo[栮|$]Rc+1U4Ե#Gteү&$7ݥ~:<shtC?ItMu؅w0-N6,,b]<+^Y7uhusI: ٕ.Gt uP@tL(p;$&Cʒ]?q(dT8QԁpZq]d#2.u@NCIp]Wi uhyF/C5Ak9XtoPGNf2f&4q]Q{N}O-ps~˸q>$zȸ,DGOuƛr/gNc$wX9.:>8\ J duCݲ^ uuCdWZP=\Gh+MtVCɮc:!:!Ѡ PGMhul4Xn'txQ:)0D+(uluc[/C }!:uvP/Cn|S,'M}g"߲^.褭kdCt+Ne!HO2:T;(uxO̦S2]u~I@mJ%ފ2 L,0#_P+-XMz20ufy:7Y:z8D[9 s2;:f T`VtxzAclL8քv$Ңȡ̨9Z%4%2]Sg2֗q\Wh3),<|5]v%:cw4d:|K1e:IĻBtLFѽYlF`F`/"040:*u;(u llɽR@*&Ox<@V'mpWLM~\M/$8rؠڑճ Q@?gS:ŷֽ ŧ$ݳuД~::R`Y/c잒{8 ĂB  r%tUv˰`j:O2ܶPCD{TD dW=˲^] u8}Dǔˀ>񡣍R:㼚f"SK lG.].t&vu^gSJv|<3[Жz/^C !>Uvkj v:q1w$^ƖWhRHsP0}DvX$29nȆ(uLڲ\r8F?YuI+<":]^D7j P_+f`ʙu$LNa蒳AUܙuFksux!nI3K$aceC3Oן}"x `c.GtX:\o+K[=\pS|%vt&'$̞ Q,\Wͷfs2i)骝Ze`)$;peO (D̽L<0R/3L ۢ rjt3 ofeb$L7bK< LZB;~I30-76y$r4bYH)=?r߮@]YLڤć=-}!Jtlў)9D8xMᕇ<(u4'}}tJ2&gV30u+l8AtMquB%3>\#g]嬷ݕog :GfU3_ЕS1{/يliɴǴ u$LL'3 S:tKx*$7|qF"0.=u&P ]uK:;=,k{sK> u( 3N\uk:Z ~u'e~ I"d \\]:Mt0h:*9P&::v<>yKT.ܫ$i.-,Dbux q1$:ˇPd)+[XbË=P NwV+x a (6סdatܮƿp\}YoW= uѡ6lL#@㧤b.[MbEV iIt&} u : џ'uPg!:8S:tvG :xN^걮r]+x+d55:#:96IzS.̺lC3 L&e2XTV9UF:'9nsu3P?[Nu}\# ≛@t.Itu25ݭ-O]dZr+JC7>%Pg:uI Muxuv;rNtW< W~Tï@&:@_͉uunqbFA]9GJ%uHtzK":<(|kF"04Ȏ)7`~"t 08DRy2i~ kB2]Avk3)5YЅsD2s>Iz埝g@ZDDO=!֊R>&:S(4!DUX\75:23 u:46Nu,F5a"֡V^g:=Qs> I+td8gı^&:k\yd`2]y@}6e!Ff(bpd:ԧX' {-~Zd:q'> _~@t+uQ)hzik,l26(qtZ1cZr:ĺLĹףN7ߜzSM4#aKdK讉U'ӈspAt`D'DS.2]pL;FigF`#0>j. uu"%źba)Ac*aX*5=sT%P euC.Ov02P &H93nL r XgZԹu24ה9QBwUƣXt}5]{,q ꤻqK/tX\JO 6G{f: jbT.$¦)A-^WH\m\i$.eJ5 xXDt-;%vgd7ZAN⩉u,'e:فSY2rK*X;Y2M87@std:( %й@ũe(q-D kNL8znL嫻\^dfKMt)ڈteG`Busb]됄6dYۋ/A}sˡ]Cc%θ*YG}re)!x;u KgJuL8>rzVd$3-{;n2i[Zu谬7L^;6p⧾I;t^Z96+o}wS%* ARmo}rҠ8Ď<2]@trnb ,6#pP v-G$=.)֡pݮu~u^zFK1tu2b궘rY^V7:]Ith)DPN_25DkZEyrs/\A%Ṯ)V &I/{-| ӭoYp^O*،8 nA#IfoNZ3 u W:X%(uRިڵrCۅ$LLb]5LƢnKcO9*Җ1R H`]X с0ncHhto9bI[p]Uvz%^FD3$= }x<2 iĹ!tY._KK+!刎u˛uylN0kR&Եfn'Lr]R LG΍+ښpr$aXB+\ɱD*յ]nlE]+-0cRN+,y[P\F`9FK8m8v؄F1=9\R1:-Ӊ@"]VQ#}G'g/Ɣy?vKE.|u(F{Ӿ+Y\'I/}@n]ݻheXm^Sd:6ź阁ٟr앲]'-ݭ,}I/tJ{J>p/OCCo@h.7nd-W.?|6.?<(EtZF6=st2%12c7%*_˕-@b˂IL71^ʌ1 ;>0CKK_y@99HG%_@q{wr O^+_QC?(o(E{Z+jPꨈ*BЛ6oo NœN돜Omh7vjtgoCݭY A%}H[W巟$It7%j9`OA,~l:s9scfn*=gU?K@$RVKW@J-pB, /|2ѽ׿m0)q#k"$ԽiN/htxNnuNY}F`"03XbݟuUN/zk7ۥʯ\g:!1G,ʧ wBw"UVC井CE㹿 {nЗח|"vsRWuDg4Թ}\7  -7뚈.9E\Dt ѡ uuu׌uCUDZ6CWIeR|o\JNKB("E>[Ls {~bgux.\P'?vb]u9r*\GBwҥ*ԡ$Ψ2 '5bCc-oM}{ 9K--2sl~ :!.ຂcecŞ :nuLQk%:[yi>X'/b]P9Z^sx柜zcJ&:4@_r]2[(Nd:۪> tO oN+$^&.o`AhcT'5}3*uI kodG :0Vu\ x|+yוcA9r4 <5 PG˱zF?α7~~~c_xsuQU`CaCuMtuek%:v?:LW9DVbQà7S?~O,\g:YA's#&:U:7P4ѡ@ӟ,#xY 4:7tr).Gt1G6k4E ~Dg|>#0#PjbEIbP_5mnqԲ:No>n_dzXy"~>mJQc߸?}\^NObCD? uƇ֤:lt_Eoi+q6] W PZ?JA[8hP-\W:Nux^*Au1Y4:X1=P}\tL@]LtuDFNC-jtx̥t4_8P54qLB뤙I@;LfFdDm:Mt-K k?ٰD`N?E0xMSs2./@NM4A]`0u>A]JNS d+x%\g!:9 uI$Pg':uC.خDgKDS9Gt":Zθe¯ZDeȽAו*WP̮U-Z@dE[)2ۍ@saE!N;:.55 hfID\x@t47LgS&й&ttuĹ@u\G3Ɓ'ɉALUk WxZ]R3FLst~o>Jō2pD|2]\%&:ityK .F G`B] ց(ά  ݂er\4: uu(ul!։ob]k%(b]nLGu̜=|Yk u{GX9c(\̽L tV\G!)p@S.#D'Ṫ=,h^.^Dq)%:p+xy*ݨ>^&  7ۜEu1ڕ:GOt]s4"A-_搦 ̱(D# zFm U/bG:Qt✴e8tUs\&)un{N(4Tg{Lh'Ujt0t:"i ҁwCsyϿu.kf0#0#P7`D-됁wKr`uvKu LK KXub3idtAq,Htb,/DiNX,2(^)SY^[ \*p,ŀD\`a,='3 /@tOkhsVNGJt&R_OLG`L3W-.1PN07wp$hq&^lJŒHAn|JXךp.#sTeuX8CuXg]R'BthT̢]3.s{⮨qbv9\Sqeu[-ӕEve5]9]LtbDs)eb23<?'u:\x"|u1mUr ЮuӔb]/XԲu=;.Y20@.Gt==JulQP[z^Ͷiaj+\u%fy[&sB9 ؉V%q.']HC٪@'D'ۢ3L'N'^.a2n>fff,Pgu+Nj2 GAcIv\g2F:KYAONtհ;ĺj!;ѵ]RkM qt@o mqQksKL<-J$mΚnjM9.|ve.*ImQP1De:d]&x}z5#P7rPu*\ ĺ\IX@Ў{`jfxBf0ĺccտToAd:v.֕ΘyD9 4p~_uGs5X$%G]!u4Kgd4yuRvM82z7{BIYJ\e{i\DG.rYu n|c&^!ȌqFYs20ь*N똁shWv]sݢKE0o,Us/2]!sLg\ج!.HYrΉ:P۟$j\w=Z9wGs^Eb&s}fxLΞxY+(ӕϵt\xr'_Ѯ xEt:MDNJ1щL.E#}w$^Qf,<#p#0nX'v)c.<ŪI]rĺV.'ѸcXWν,YP3[=e`"~`&^XKELawIa~xZ/ȩ:_\u@Ǯz"ggQ2+PBtPDG{'C; x{%d:TD\ G#!3L?A&X\Ys9C ,b0w4yA1q].RvI2rݢ)) 6e|*9 Q#IU#zSLN?vxE W$!edxg{ϊ33OpO_~D h~G|x]R * :}Bu&Est@S{p|DWʐut 0(,' ',wꏟ x6|2*" tA%wI1\&cp]Έs {`&@N+OXcy YL ,MsgKnnj\JΥt0#0n"\f.h[s" |uxMs/l& XGk)uA nǞmEw%ac醋rq/z':ut2uM)z7|x«Oĵ3uv\Ag8jʷ}6S^ኝź8!xIn>fff8amcgx '$0! ?AwpNa]s̄47r#B\1!:<ům;bXX,T Bs(u͡=T 6p9 ;vByHӫ;!HL|:؁S2=O惺;7(ywaޭYo>,,C1K^%e:qgn=g^l-D<Ҡ̝z¾  r闺y+@+rI;K>b/)T( zeՂѷ9;4=P Atq'BI@&a>WΡ vCirsDcēꜴRJ|xKBp̛^YD;[hs3.l2-K˥t31f ukuvuI1Nv\3^P:ǐX氠<黙nӜP+:mn~"I0[g eS{٣E٣#VSWbU#%NXr蛑D4YԕBs:*&O, Z4 qxuvDKjHs]Rs@ʫ6oz%N"/ 1iD96CPLtn(zOѡnD'ځû&U?1ꌁ-V庪X'h'\;ԁ Z7EnGC'\pb<ᮛ{& '.,|"%+ZPCt9Er|4'۽{]\7ԑh-r_xKM\Cyׁ耂:8:tx+mź,Ԁ S~3%usun^L_}PFJ]!z|r#:i#:_ ?.$֔Kb#ԁsGiBTź(MJnRǎuOf k9O].&IDAT$)t> :"Db)DYJɏlGD'Off_CR0Ppyputx rDUŲXɎj:np)Wr79{]!:{&:`yeͭrV/Z.r7WItM%\)-Ib]sMz$ΉW qpD' r]SÌuI.9I. 9.iyd*]H!2Q[\].'ZK]%Q&ѹ{VE`B/n]8rXu:ADr6p ЎB+uV leKzU6* b!l .Jtol̷2Ήue.sXP :hresѱ D-٘2{Mt^ 4,a! %Ż\bUӅLt-::uƉWN չQ\ tA[c8'[uhxn3,VXJS [ c\*BwqNd\֥tBtqʥ}_($\b2333Ljd\&ĺ_@tY^b:~Rs8sXn ֕S.;n[)NQˍENV(ze4.'-u9.h\ך&:vx=-qY2q(@Xk% ΁~/ߒX+}DWH5t9-Jܖ&:rD%.XG. |ߦLgYlF`F u@/&\?pL#d&oq"nXtKUK^ZVuG(cEJY:n b]'э B!2uzs\4K]uU.pS Sʳs]?|B֓xWʐ/Dq)]0|Is|&\+ LG#(o$:ǰ*33LhO}_ccpt& 0ͦ)D;jgvJp]GU{+\ĺ8:a@(F+d`eBw,hKvt|˜o3 tAe+쒒\AW . E; x6}Bts;%y_a_ą`91"td<it(VNt.bHtHDItqeffz"0'z?vi.Nux.ۥr[:^DG: 8㡴k„"<27IwgisHC)?hu,kaf#>+mD~mfw/u~'B%O}r3/ȩ)M<7?oqyщ.'d]w|]..^#ūq~{«/Rppusꜘt %X;g$:&[RY V[^tFw?yt]`PWItMp uC8̈:&lNKy5q}N:$+ڡ%aDG;:4] uMD}]$qV.sILP*D'ꂳz/10vn&dcC{ڑ:URb^CY\g.a"%($i&.:=N9xEPwh uIUc1ps]Ye# bQ `.v*ѱu9Y/N,ؗ_?yi ik96:RC Dthщ@bF z׸?Y=10P7< 0)֡Uul^gc.)ӱ]r7U)ƈXg,+[:P'[ urap+ѡ:z@Z;;qMo: ѕqN&X Cd*NZOLg7 =Ui bX\WNL~ozNMB]keh<,Kr6DĬk9]YCrC8G@tN.At?zKo/u\JW@ͷffb;̲p,^z:qr9 BJDZA]@t E+(u낉3o ulłve:^!- bu\rDo:@;bHttChBtiNCtvєg]pADW %Ѐ8I1ItZ?Dw[\It:uD@DR)l[0RCEQ1:O,`X3d1!:<ڭMFd*sTW)trqQ,Ǜ`^tyTXuC䃋9rv $:\'5yU@״4~9J@c< : \p1O~É@2IlpV5&:ǾJ`< :"M'$:t-˱y⅔˸:xO):9@ ]YDʌJݚjKu0Oa&`}װWrb],b:ze{)quԱQӹhL*uPPG7z:a§vZrIN:1Xrb]LǶbVĽ E0Yݓc+uG}^Q8l:qN7uqc2#xĭ6@K>GirPg洓:G| uY+x :nXEd:@*Z+ kKD')x7κufengf u[ ~su )㺛cf]s]U+s]LthuhQP-\#:uAf9e@Stcn[2xZ. uCS%\E+s]+mrQ\AW*pc]kQs@qx\p :XNeHwFhI9i" V+ԑ8& +I! :DDỚ踈n]S9 B&(40  ց:n@C- DZĺXc*Ht(%YD15`%sk"&&:OwNT|=uu:B]nႆz u,l+RbiN!? 4G1pyBtdKMt&xD:q|kF`F`G"0nG2"׏X st*܎)I\CjU. uX:ԕ=MuMP#+L tJ4z4YN\x׃@1I4ܒ?YQpSTGˆlQ΢FDAI " HWJ#* 6٢UG*GAPXX'vl"|\ssFFF<}uMM]Wh^〮y>[pG3R'fN]hN#nv4'JrNN>+?N0'㌎L^ 3X?RGgt .uIo]|t8\!uA]6aÔ9Ҽ5e2+^i0:b!l^#u3F3u7$u$ѤyNHmIm I]sEd43Ou9Ft~wf`.dpB .Lft92:hN$9ret=Q ԎKX!?kv@2:3:+^B+8n5xMԮ'uf"\}pw # ^WNh:n+ú1NjB\Nݐmu!Fs5ԱfvL4 ft$#u.㩣.JΉ[V.3$>v~>}C-#=yq^b(KѹGs?:GzF xc7~݊0:zH<#s8\!u^65vT:8uQú1ϕ&u4vxX >EIɋze4zFG6:Ҭy]!ctbxX4C!u$u cB'u=]4z^i"9vXE/%N.yt BDN3:u|͡<_/INI1:Gm:c6:ftH1 ^tݐ(:Gcs?W!KۿI?"=_ <9z_Ԍާc:W]!92 8LmE4uZM~W=l u #=ؒ=|Xg \: ^7'AǼ.Dh uIJ:K~k8$o$'xg|xeځ=c/z^"uhOú, CXaC ] K![U,zd=H~ʧ?qb,ξqi3#=.2slA#ѽ{>8y=l0:$ߎA pRwQ(x6oz*>xlaݐё#yuӟ}O7KLx%jy]ԑ~q21_*_&_xiXv.:\NC놤@xNH}ѼN&EA^;tͤ!FG R;.>:y%؝&uќh0:p  _o):1:9k !RAߟ{;at*>w!uwa #-BGыx݌#RGͤ%H"^QE"Ɍ^G3{u_]SKhRlR_fiۼHf&uI]ѱg i1uL69ut bt2R(#3RbtPN[3: Kwt4KhQ(1s$c0w73щD;C8z̷ud S}y3:mO|u9Q^7#uZu:2%u\^g:uQFtnkI 0uUtͼ[r.AH@F5ڌ19b:Gcgnv@GCnHHxzK~ !0M=8һP:>]ٜћr5&:/uv:RuD|~~_Ym~ܗ&4>$bt$_BH>fGf.u1]hnO4u8c:>}B?kYӘW=l|&:'ny]]>1:ِ:ѹ\ 1:fwOߤD[@:GΠs4d-ټ5t$3ctDS[VJ{8\!u~tsSz^Gȥ;S;~ 3v1ɑ:uDOcauػa>g!ˑ:x݃w@oV憹uatr4t^5[VmnH}M >rf~$~vuiFG 51:! iF'9FG4 MhHft$0ΑL䀎>m<E<ή3ӃA8cuWbCqNOBOhl@׭:u R"w&e^G/T#7:u빜بIY~^1Fo~*rQHt(x(#FG?Mt}+h>.CˠD #;0y\i[?u#kHy0:urRǣ^0::ԑבA4&r͏/·?BLռ!u9#{F%Φsb󺆥_}?g|MM F`zL<Ү.DN^k,#u$@:'[BfQ{|\#;>Fu |^׃^˱Z͸+]&xQatΥs$]%kk\oɟFG pxR#?u0LV1I#U6ƺ!u4=@>c7Cu:;uÓ\UB裆׉rgrKy.ԑ9mgՑnHdvui_w+V1Q;\]H3>s~ÃMхFg\of03HF]@G ӗ\{G pxrRB7Xxo~?͚ב摘ܳ׹'uu6:eѱ1:h:ԑ4x q(70M^Ry]3d.GE;e3:_Gb9yׁtNuzaUGvr1Wב|EL腳;!\1:r_lfLO<R]FG2D쀎0#FG%>1:{4 pxRD4u__^G7Gvts]v:ѱ3^wH vHёMDU2:rtuc#:'x]t>otz2: <]'9~ iI1C1怎$1Goʟ_ѹ)wGQ-JRGSu:5;ѐa}L c:ְ4 9v=OA^'z!CHR߲Ť,:ѱC^WKK9.w]t9^9'u.mj~(!FDOX!u)1ӿK↟F4茎t3:bt?.?=\&aw{ta}L'f u4v RGCz^:# !^ל}=>u ?":u55ftPnfB::MKy 35)td^o9!tN&uy?҆nxM&34AF7r!w3F"}ثLyi{O2JBFwܸA !pH VQ^7)M^:2`lRGz^wy&rthK:k_)n\.dzhF'a&HMn]Gb uf:!ͪM~9#F2:,!FtNӹ7//.c:~~娚 HѱFʽ#|8?vY:n_:"ut|ϏxgjE4I:1ٌaxLa]5%r:o= :ԑ^'FwCR7X$=#a.#tNy]HeV5ctќ؀3Α !.:Gc׽#yjGs$@W]Mt_>2u'!uO(X[LejäͣS)&?ui'.x6:Ex]@u9RGi<'rѤAU׹:6^;Kz\^%u$Oj?>>b3ӢmgII)u>s??IftNG=C9lBBb~};g߻hp 3F'bLe5.X?}t/sz:GN9toc:GCF9I9zF:A pHSv;{^V3㯰3RGC@^'5x>ׅHMAS'Nh73K_"{_.놤T!'u08qt<&u%[ک1o}|ǞHf?(cOzIPz8pB>_+~SwсY6Snx ݐkI_lIt~K}@GrFKѭ3 p]g׽oOvGu=cs?v V_p|g#1vZi[џ=/|ƣׄt~'rZ:z3f.DHaC!LaBHHr47 L1o~=xӹKs}zL֠9a5t3k{jnj.q4'S JKmft]OHr4#!c:; w5tVu8~7ȭ4Ly_)wzFg͉3ݐΑf}0:txɃAI#pHݓ׉v>~,ޛ u$x^bw;]"$R(cR.zRGzRG=s!u$u!RG5kH:ޭفtNOWxdg9=cՑ]s@g"tN+'ju;#u M7dt.ftzFБL䀎>WK~30~wC^.żN.\131XuLHC1y!GvѨ 񺭤,NH!:Pn=#1EI)^Ǥ.z.AĞ#;ɌΥs2Q蜨Mt2Qs. _? ]8ӌs4ul:GC:Αpz:G2 t>Ռ+ϑ<^l{Qב:9Ԏj8#^א:uBh,4ȑ]CMv՘82:=s$4=1:%:W2:֌cU"_p^BI#;bt9-gFy_liK1t19]3:;z xF9MHX3!#j9zA "#pH݋u@^G!\^ד:uX5솤FGv 3He:ԑׅHpO }Ii^g:RR*:'Dy]/8#;u]o{d0q\?Ws@gWgЩBHUć9]s@G :zS9r}?!uO7vYbGa+GvM6:RX I]ׁu6y]5%uPN'1:~~:ԑf%}_~x?^!9^<{bZьL|TS1:u=.:k\9;v6sO4<2:Vu3Fg9vBs5u8kvxw>}di#B:g٭9=KdBDs)f@g\T-^fsLO.DgF9&Fǣ-; 3-Eї\s/*Խ-K:>W=50Lt:k:ԱK1]Rc jw$r:4$uy\RgpfGvu>|xz:w`69?>:EJ蜞Q%z t?*33?w_ʖ s뱯dftOx쀎?=\"+/{ĺSv|} j:ԥv:ԑfH!gP:NH_W&J7F!^::\NXfЁ|E^ P.Ma|d=+<$a[‮r<)3: uClb9'Fǣ F7;Q&0%!u/FuoK,Zu#63H5x{Xm_xqfGvC^"u\[rCRG.xRzRrzEuL˵|_gȳxk? :/G_?|Л_ɯ[\Nqu¿~Wo3Wu2Co}#g]OM:c%=:zF/+{YY9kבS;z1ku #%[2uc ͥ!^GÇԮuQRG!aqa_K:}nJԑfuѭs9Zjy s+ujg'MG>p!74BW#?s Usղ1:rD yi#E~rF7IetBH]c&xѐ4Mt5A I#;uKxWxNxNHό!yNH>:v K0:V˼nHf,&uR uu9F=TL~e+̼#~ 9#:?Jp{yujtUiνrlCԁ4~ی|ԫBO$ٝttgַ px8\мh퀞)>#;6Wr4 yHhxB#c i^BH-: &Q^g3:u뤎(gtQs:Gvr4g vGvC: Xv=͋Φs~t8ONR2:9:GSNٌNo{7 uԽh&v|):97~Ϗh/qyNHy:R"Nx]]\Q")!^:9uQRDŽA^zDg~|އQBBξҍKՑBĘ#;iWc-{$ӤszRξYnHfthutΑν~/b^6٣<\&cDǯq!Խ ^rEv8#kV]Bּ.GH _1_EdH'~IC0>k?% G.dUOh8@RG:{^3:r;s_uCd/^8Ej.D+Gv!:,՘v!rtN8{ftC&% FDžJQ9q9RgNIKh0CB/+{Y{~۽'Ej NG4:Hx]1l^"u$F?nHh"ጎ-oxK\N@RG- ^WHđ]ɤ#;4Ұϋu:'6Gvr c6#GsŐ5Gs,Lt~Bs)9i\ut^.|ΐA!u/yݛ+'~/Ťi4/Ť=FZzo|5w%He^}ȧs<7|FP33:(f.JHuCR/rѧ!^kC^tN٭9=)Nrt4R;z^H Ku6~FetuCFg9D7s$6<ӧs$3<ӹwz7[9 %>^R2 ^ڹudȎxݻ|wr\m 9C^/ud]GvlMs^G̞B>Q;?ߨab^׼\YuLHm5%uu9RG:i djxڇr=CRbGv\׉R}tN\U9>_4fVb?sVIMIftќ0:ѐ K.[˫7y|~8Rw2c <͐0Mx rt+NHѱa CHiEFvFn:)u>k%~:^w_@紵rdg95 CO5_I$O4:!bCfw!jbt\H3:hN,F7|vs$i:G :Go z3P8C^ }}):Lv$/}>p?_x_+.4#BnFH@F.Dh_eO|-Dh^s&ڭv 'ttVҿ'nHrmh 1:93P&eRG s_9Vtd4 %t~!:'K.`A 0C{x^Go ^Gzew>Oo3d]5]yAHB5%u QcRGx}L'y/c'|,/ȇy?+w5h:IKOQIDP>]D$'Ήy+ 63'[6/at #s@N3 p8 Rtdhx#!.c^v3KP;u]Eyݐ;b(#mf r:ћ; QԑS52}dtNmuyk_oٸ"u}^?x@g$OY}tx*dO耎?uUr9z_s)3:}%)gtD@ϐA pHI_Pzz ρ\a6/ˤ#;u40wpGƂ_u0#uu#=3^ uy]CҤCGv8c]hn]푝>͸nFԮ?iRt|K ]n>9soҟ?f]p_cF7s }Jg-ACNz\)dAEv})& Nx:kxkH;`t<@RGzz^ctlmrr2'u뢌'.@~d9=i!C蜞:Mt9Gh9md[?:7lM^ڌ5FοO :G|FG/޹fxOH^wb(K1-v= !v:ԑ3^3:u Rp(0?Ex]ԑrҗY" #(ȤWcF6)D:љ u3F':W]">>K{y׹tP~̫kzF$w}iU/WCt<YG p8T!pH]GOi^GZ4k|hvy?Lexd'>7ټ!u4EIѼ!u ?'3N?s udҤ3W90CbFf6V蜶vh K;@ j8kXQ8ms>lFk&F9#^njyiG όN倎/9k;.Թ-̎^g~ao\i:e<sx^'!w6#U@RG.AH&!ћ R;ԑB)QR943~Zd}].Ԯg9=8s%jh fIEO皗KK.{.G:9c:GCd^[b5._W:gWA p"pH]#_ȮvuƬ9t=syݐጎhx]ԑH?5Yr!u,=sIN~Vkk (7<[Y՘;蜶JSc% Gv}7N nx@7r8\gt>cF׼zU?{ӷ~%czF,#s8BR#:#^G4v61Gb1[zm3v(%s9)\cCHu I{^璺!4[`VPY}rdhnfnc 7{;FⒺ]?5tS.QCR\9}5q@t ;Ѧsks"Fw蜑磃A P!u%0%hjGؑ:~zJOu4ܠv[7;N75.AH9:M7p#u=c :Rg r)jBƻ/=)0vC:g1мO_90{ɟ utC4cF_q &,9:}@r9R2?;ΑXtΥs4Ѕԅ:yM&Gv>#;!okEylIc =((:2 .JHŮ!u cfTaװW}k'}UMeg?ιT8cؙйFvF_Zb03:BV;cαBM!\6t3.G:Ғ Ŗ膧s97IA pXDEbyW]څxivGfn$z2(C u>K:V+N?a}Q_푝 ˆ:C!s?ůaD69Fj˞a>}ا?,o‡CbOgOCABFNw2K?+-Eat/t-鿇ѹQ;A`C1<R;_L.GojuS܅nMC>y<+_)̵ #!^:R8uiFG:+D4ȝZ$_rݐic)$xBjйTஉTkltNvaAFǷGs\3heK.D'3:>g:7y p8\!u| ّRMB1G>;`qs3dRG Rԑ!y] #YFԱpȮ\Ԯ93̨v :Ry6% k9-v8kh_ϝߏ5dwr@gͱ}e42N9H4Ё +(م{dG yjٱ>ʩэ1=Xt 0uBXmnHHѱa-vkHQ;z~IׁwٗYjLiڥ\sڌLpt$-K\:װ8pEt0:-г;ftB^Πs:> 7>:9$A pEZDgȮ5 YpZr@7zHtnv7Tҹf;zr$#d~J:}ݐ~y;CVّ9;8nGg=Cj){MONrNܻ7=|4L]Oh8x)<%H)^א:P'S.AhȮhnZB.w47vtn7k:nA3ED7{3c6i׽{=^2##a!cjGWؑF9H]Os@ 8]C#;uo;^lB ϦvP~dא:RIۼ$uid uMso\]i;#u<=ˑ:M+ nJKЅt%l4Q :.AVNz&V=_ Fg5DNf$Fg9^v :GS:pA p]ڱjvLMډA=cjGvfP ?IkvlRG ^"uJ:5>xߩA:ԑ] c"A@~~염ׂZsa =ß2o=I[X2:v3:7#r<{@G21sMt_Zb(;y88 ɣn2ډ NjHiyKH(#UfnX)gyڹفrb:~"Q~$f!_iٿ>k$ c7=z ƒ!2.MXK˽~-v=;׽z?W4D_(bG p8F{4S;}^F7ћi^i:zx)&B.XbG?N@䁐:~?td2:: ,g5`ڰ1y Яjg9m"+sM 9GS;>=Y/:}%ˉ]s%B9$LG p8C 5 ]^ǓNxjJW@7;sIcRLCMyz{N"GvQRNH]ˉ3RrM\ _k2A4t50HXsh9r:/9ss:/#y8n!uB̻ LܔfViRj':5dz !u4~*fԱ #[/@N(3HHi#;y垠JhrMPx4g/Z.A Գ/x*⳿;/{T vs =}_z*9- .S[rb.G/sS3:son|ݐi>=w!uwcF1 :եv=QőCځf#;1l6INlRǓGv R9ҏp}NGv-sŐv%tNIDn% #1.uٓ0gGs VgCq=~n%.M紅v;skv79J".?wu|@/eNt,ws:s87F'c|v-_o7m $!x yۄiy /!^ן Gv6#c#(=?E] O{og Fm3.ajGsڶA:װ8z.ʅwzKse:A p?Rw?8\S;uSS;@< BF!o#NHM:]ٹRuRGV" 4#u<֦v8\N"R>t_"xijx*O"GRt?-ƒz>x_v.DN? נs=hѧܡs82AD2)}dG= 32vD H4 3s@R4Wc?^ ?v.r9VL\OKfw%>sס{YKl\^BHft?kfr=Jx|es:8/Խ`=R;Rdxv ]jד:^>iӠv!RG#kX1j5 Q;t3TN,GXoHX!\m-9mNӹs#%|澚9Cޠv sŶйPj:ܰ;}-p. uSAcF3Tfd_?܃z_0dtFKxj(/NH4MnAj"uV58fAX>M_֘&3zeh kFm8N1k^ʙ6D' \C]S=V?v }G p81|j'ԮQ;dz$#Qζ{|oV|7WQ5N'u*CRg9&u>S;zwQR'\>տ~@&s)Y:JJ؝qel}p'[6 HGsUD&r,&t9%B4O7r\(A px8 qg/L~y8ߐT=geFI5{d~1߆1㏢nFXKtʑ:ȉ=ȸw \N wѫ\OKͱfpZx4'Czc:sC9y霼\tr$p,1A!u/XO+k2Iv0xn]B䚓:m}j'D1IK&؝&ulNlR j uSљ\E'MXA/c t.כԐ:X?c:GX8hIfw釠4ڴMW8_W{s/?^;hn6{$Rsvrs}.K3 p8RB8uT~[dzחɞLDȎI R;r cvFBXېځW={<}!ݐ Y;#u,8kivpm8#йF݌ 3`Ylс\+-<<!u'#^lvlްe44.IE݌Աᤎ5:q(\T"u><ᤎN:˱i݃Z.+XK?;9 rCW>w_O:<,CBrC^?#S;aw57vCRǾnFxMlRf.JXS;z4gqCX:vQRI*bK#qRJ;!0R$O7DƂGsCÄ5M #\aqZ͆Gѽo}lO{t!:Ae!u/8#_s쎨·ӿ3sdř$_l}7vc{jH$ZkBۓ:2(Met4$JeawH~Ga*:U ;hnܴ/sƁ>3ܯW>xp9$A p8ԝ48Br8`w&awo_S_jgiǚS;NNXv/x?T~{8CH&r}_ OD=K34sr~;hFfGs3Y_b.'/ gqͤKs͹ܐYt9CNz"S;oT _~ tOEI둶W };Ҿ!uv91[vdw .Dnfw .749}KBC3^p9ʼnZ9My;s\=@C! ݽ[g_LKىL c'Gvah@Ƹ!zD:\ƯGc.' w aw; 1::Qް;!u\ɟ#MZygf4刡3&..Gbh.3 p8 ;q=CvGf5`wÛ&xr;:b||RԉyBXaO Rװi[I~څBawMgrgvyN, ~{1F3=^fD^\y* .w۔>C^מ=xC?0SS;F x]H\fc;Ա6}M gq:o4fw!RgQ%YB5p9tw>"X~.r99W{zeX|8^{!xxhIN8| 7Ԏ {pד:1fw6%Cv&u:y4$u sI&r,%WNn@+w[?WD!u\s" ~o=Vft!r?~_?ye+ǡA t8XrE xQLFh^Av܁2gNN鿟I/٤fw=I;sBf42A P!u] NIIv43 qܡ'g\akd ElL+dжBr=녅aqr hᝓo[3V0DSkyxX;!uO7vvB=olj?#!ϽJ;;onrӯc23Fs9GB8;dU둳̭9F o7΁Ou/eѿ=8#v8/ Խ0<#px]S4 _c;ia8TUUna̬+,O* m=:)WawpC en!M TuACfjvE m3{ȴ~55ĩ6i7i{n;mM:׳ɽtC<h<5v/T[Re]z}-U^G=ptU5!O:~^I>\䐺b_eiwۉptfr7rѪC.Z2 Ť[kohsX_r "/\U-Ttgf:j[uoYKIpkk+֎~ZmmϚ.9H%H96Ovh;wErՅ6zfّ:ud ل Y:j0e-/EImy=8d8 /M4TpCDd ^yt֝* eD["*0O z_{:,fKH;џ՗JkɁA ey] 4Hİ'T]}"0ώY5%K2J1 {D6-uD9eoxP'`)z#slQpSK"^Deٛ h%{/psXhIO~{N~HBI]|c׵D㨇#?NI@g:;H^89[nבيb^N/`h=L>-d3X\9Zs4tn],yј#'8qVMh:ӹF\w]rԹDI"Wa}`4CѢ(Uco#8|ĵmn8*nZ %7Ю|V)Ayw w,i>"k5D,Dd 7nQ7ҺhJX)Y xӦz=v^!θJ~ mx2] 颢V%MZnll#דY,i/fb`@m8iXrr[upb# 2hn1Ʃu úR1/w2p~D~vސ#Aݴe{x&gj&lk+4dv!˕pk\M7j^o~'_ZG7,{k qp9G@+Q081$`bomjW H]GbZY+cJhR-O$"7D j] htlT ͧ&5hy; wd>֪Z.}9( =M8k^R%DKnBsDv5vƤL\Љ2d~Z,Ε!Hbj񯛖Ò@jBѴ̵5"CMR(U7\1DsČ;L!ҒE:í4^!UbFbCj=-o ޡs%͐܈Bz+͡vn 8k߼VMeLBV"c/D|W 3 3p3@bnU4!&Pس%bh^IB7}hh\_pҙ2>v! 渑ÖMM:x[ݳ(Jts#ꔫPfVskjX͚7ϩ](@ᇓ LT *PSRsuLsG% kb#Z{|z=w]XQNBE0bUp.6f9V!cҰU]zxp T<עd˩<,c1uK\gY`}~:{rqi7[Lf8 lIɕYz8a$l[b b⫵nGv  5,R|\.,7oYX,'b<9rrmpCJpa\ktI]JtJܲiWVסvUz:7Æz.x_X媭_;T:̍>y;ZOebt)V*UETD쯕;ohU_ʸ`6K mkG2 U3"-ۑF/dh:ݺ +W8 H'u k)m]K =eRʢ\'}pDkkl D!ed]9 ۵qH`45kRaWM"j]W@dMp%&;bSGfH`πB.,:gڷCB锹Ν@ gXU+ujtXܪJ^WWz%ElFdSalSm%zq3Ŝ'8C!Qlr>.@ga&unS$4}nC*WS\r.7>nS( Yt9mMk|NmLQHayqJV-aѹޅ;imWR $&1eL\02@%D7͋Ĵ5=b17ZqW-*4#1o(vԟjc:pIVTXYHuX KTw mw~슶(mwZ[Khc^lۮI#y|[ 76fˤiAV,.'J\AkA1lC/v<Y2Ȍ>>biJ#46zhi]]^)ڀ5Lcy#ٳ %OA4@n!O}ڪ-7x֛>E"sy1J i _7FgE}0K\(0C6r*tznT!6< r3mM>CwJRXCw6lH ә;$+[@w|Fj'MI]bU9{\{XD&RYBnESjuqmw eieY =njZÒ< `p3#8\g6\C\k*7Kml]]]lCG7mz &̻]q^÷&RFOb)^)b$"0̲^1`ntI4'0v=呵Tt}] IwC2 FR[d%Ape(kvف%Ga#0vq 1ߌ«ݸd5#MjiuQl16BQ%$xKc%\HfEO]4\hG_Pm]![dz+͋)7í&9nuyjӉ~zxtG准EՌ?)ڭ1Us'c1"70LHص]w+% !ޤZ#\C.3/4kq%דSe֦;Pѷ.Un.a/sj@헑@ԏ6ͼ#c纬" ltM"h,ʸˡ3zOl`eN[Rp3} +b(ѝpgk mvgDU{ nlf> V~OG"ȸf輜y+W[]t6{Hߞ]#6({8Uhp.ܝB}Anx2:w[ArsINyԌ2:7dUk_|A8%!- T(*?I#V%) Pd1!֣4xO_uk*'R7&/Oj?^0nڹ]u] =}Ne]j,.Il%ݢ Q͋13n_vZhzŎZq]/}+Ļv_s)D ?E'6 ։BV%=jsU嚮{Kf aHϢot6dBPWNu;}:J/kD. oVzk5XO2uRJ0 HVp-5iQi݁vOz;{n=&͓N%7!_fMձPZ:ZBS]):銑9Cdh́Y ( l}3aa_>?׆2d^G(byç߱; ¤.:Q&܊dZ>4i?mTO9n}ޱv ?\.l*DO+4&]J0]zCWtq%CyR=31kh$(&6Dcĥ,&.*Ͼj}j B ע64 %#djwi6q]m_HuK5)k ϼ < ŪXC@ma\iJk*tqr?ۛ,-vyN]석iRC$dbQn9.p(s۹!oA F ]@fti1D'.bY|=H]"sC]GtAڗSd.l,ui FڤDaʕ'I@cZtx-h>P[m5Ko;-\R?mOP,@&]m\S]>MXC -FxبQ;ihȻN܁MiZERt#z쪞VLȄ[B?͊^y]2>4jVʵ5B-~A"+/~g $`džPbj-a5K7y(\ ["BEtnɆ ӵyBnS4T[phͨ1E`=+M,=ZEnS~E`A b3>]bD2kHml+bmMzQyJ9n=PAtwܚMEAʴEa+WVDžq$nU&m( "gum#aM E#nO%1Vg遜$v2b2oaJ'8E8Ph\PۡOjz:!WxPpBjFQqcɊؕh+8 /$HJ#c /]ɄCwzSD Kj6"dv~ ]B\#kb'+uF:P%)%uHsY]Z6zYNLK;p3f[/҆|zl.xA^lѩ!XBsUڝ:4]9y N-fWBdzlc6h$.+֑'}xOvF S76|c,>| ٺ/Ь~HՄ؆,F/Diֻr JBoȵI۝8b@3i†;J c]!$uݱ[:6J; ]4igMRMP"S$MeʺY]j. 6 \IŞ_%%VУn瀮idȾ%cz`bԈnJ@Pn%;5(P)+?@ܕ`q/Ę+6Y/};bav5E9][CUfh=ѴtgM0%X /C'ɧ/.Dn\ osmG7\7ڋh4kM({؋ (w%jKhXFȱ5n!9/ܛٚ: ދAz#f^^7&u8"nm5ʪ۽n1fe;j;oa Ћ(!%PF=i%DQaU;+ fd8#f^|_P V,*\`+VIs'$B#.nW3ñp7a_j*P6ԭށb5HK&!;w k>>d^kHbSL  "dMC[`U+|yn $7K !׎-nfG\ +k ka@S@{1d "#i'^Q[AWU4g amDy+`yd nRW~b}Ǡ{Urw+kgH>|xֹ)Fs.Dy/(.GڠW(L2}e6E5 INj mJ'MitjںFeSՅXRRm/Jdd+4oToUztt%ɀ+a/p_\ U!96UuCˊpaplkg7Uv3"L-\Z$R%JzĴڕ)b'6\FJkPLo>GR'yĸo k*r"ժ]¦E٤wjbE=m]@_C<BL,~2²Y"jt.|eY%)ѹi m!N6_޿.X#+aZDn;QKD9i#,hD%ā|r}PJ&RÐ$F$sqb̒0 ߕ ml60z=C3jz3E\Q.BfۛۥEW!^/2Fq#(1pFi_F9[7Y} KbuQ"Vch ($܄@x]1[٭s9m׵F, #;*2>u}6;%+JM0#4mzEKȸ6ٜD-*Lר͹VSjvG uFmʧf2!6 +(E4!YlF.Jh 'Iz\i!2 F U{c=JgTHu`H][LY鉅: خ7+c͛60 Csq:Y"qOΒn/ Ll_#YzWh-B=0H 2/ l p'zտI< ,pfFn/ yn !mvݿ$F~g^je"Yk8H!\-.Y3 ".vY![0A ֺ)>掔hA[AQMZJL7`U7zpJ>$Wmد-Y{މ\Q|nu"FmE7|F\1)=Ik_e\-w3k 6.zq$O\K{ީ [K(Kޥ8SIbKC;\dQ/CQp-WD"zpsL3ʋzRf!w2\:njqGR^?g. jH]aZhO$їlZ A )VKO}\ܒAFd  #*Wiv^Ѕڭ5u޶媍NtʻAo/CZ Ctwh C2k+w jg 9[(=̶*; ;: ;u6Ѻ#*h;Cxh l%S o& Pօ JR68V|3kr#4Bxqsj!ml2m[u -!io+G[+Vbxtʝ7&؝I-H| eP8GJ9(5H8\,;dgΪ|c ~k_pRgC  W19l&Vld aza߽u%yS2 D3ܘAX$!*FdEat+ 6%ۄ )"0؃+qUIȯY,ƭC܌F3m@ܭd}wצLqnԹ+,EZ>Ltn9Vydc1/9SBIѐ΄z 4oqp܇TpZ\}!c܍JjvfIhp? @-hXz(ݶv2Y^@R/뿤bJ5_,+!.iᦁXD/ccySobmGdXyD$rTIL%j3A`Adj7e[3 k!S2-a /48u.yR:/( #K{.:%QoXI<sԖlTUsSkf& C87*:ث/tL*PJ+JwjjC6@ "2.bO5 n)0dsp|6pqzE!ː:Y-^܂g]q}kŌwba5XNE;Vk;EWbhc$мZ1|H4i46ՂakC\QB&;WZ"s`I;M 8"p\& y;A &MQuARnŘP":5/qXq*a+o|"nݦL'W h!SfmmSg7Z[w\x2۝b]ctzo:1[Fі(% w-eبud$wmWfΜf+-1*,@æ042s5}WFd!ufe'䈑BSW@B^vIYl ETEqBeQX̸dmA'Ӹfx# fAhA]W({lS7T3mooSgٚ'҄j :jntkY̱6 I$Df% why$w+}G܍\"Uٻd*]E;p^瓺Y |R٤ȬQu(Y,MŜ;Q(\pj\\{5I4vYH4݈ۛbU>̰2O/>wT_Z[S)Wdq[9kɊnvD \1ҶbD2:KYj[aә_E쩒:ߴ8p#;S?EzRpA6Hh;1XJn2dOPYÛ͝Jz V Drŝ5+;)@p1-n%K(-(p08)"v88 $(6L􎐞tS.m09` D!`LA<=umا5hq-[zZysRD%nDg܁n}bH*zt Kl`I{Acg4!gY 34f^<ܰZ#坫NmQҟHP +cdM/nBV$,4rvC6/7p1+k$gbo.v &t@60Rpvd CN -H] DL#F,I"7L$.;z_\܆-3&X/3$v'O q{¼ )NԾ8ZpjBY1t.q!!]aD-/E~6ڶ-w#XDcї~x-t95-90 6-{΋a]U '\41v1"=IHd⮪U&O@SR 'y'>D30B-f$;q_X2cneחn"Y;r]6j8ѾE1̖Mg'ɸX9$#! zm%H IR( Ț.a^nHɜNxp'QR=TFBƇu7xA$P`jwtd=Ixuaϳٌ"-E4r{-kDLm&$!X\+5n(BÙºηs6*֜pT9Y5{6\%镻6yK@+q΍25/*ŝ 8[ܥGݢ̢ilZ*yeu f`Bx!a݀5@:/*qʵˡ5U~ 7`";-NuxK!֝j`7!ʅ6`U!@-OV W.Sk7GZ4B9h[CA@"W`hEfpte ٹ^r6QVF )"sn9zmf$7lJe2E,"N bn^UŭMͺhV.%񜼦bn|7mޤ |(Fn4ͷs"Gm ϾSLtVFw\/G9qI] \esa(f͐ICS,9\h0n뜓:{eڕtupbJb5k %z> \D[xRKI\TC p۲P_))+)أ"[lW o"Z_q8$׽ƅ"P>'{].f}'j^UQEVDԶFg?|Q|"7D&a@bulm ]ctJ=<:7rn-s:mnܕҰXBs58tXA/*ρeeqXC% tHkna%*CQ0M+G&Bd6\3$(z+ %ю"=ŘScۥox-rhЇIaYWhoRzՇzF>()5wՂMG)22^I\\ ޗZqNDo4W,x҂j1<[`Xdy+Ñ̎L'lHd"]%*dӵiPQur$m`; xj;J7zz67э)]m$JsPNeQrWkz52uNDa&)wAVDS7Wl 0( $T5>n`XD/楍1iny7 0$ ܯńJuCUx%Xi;l} Mm@&]t#Ёͻ" DOBcMSJ,vA!+8%9Ds3\ຎԁ%}'B6Ht )|څ&xdXWhOoh".ɟЌQaAIKW>k8B f\,1Qm%% Cpk%s禈V~adnЯhp1/ 2yжF()k}Nt6 2%juW$J_pT!$2 7g͜~Ԫ^>g纞i 5M(1oBK2a8%n`Xa DN%H̕H<%lzwvEEOG\]CkQ6HJD!@Ӿج4ծK̑ٹH&;HBs(!=416aab.8}{cɰwA4vFg_Ty䴹6ͅ57݂{ -˱;X\!ƺ@"g0/hz/n0%H(WUrK;1CƤ ͒NT7Xju}Q] ;\w- Ў^b7lX( ڍ gsJ=jsq.wZh|@zO0 x.T3T֓)C؁OΕ&7gT:Jg[Jޤ;-p_iv \$:!ۦz4Vo\F a;\iWb*sCg5g/ۖ2>#X SHj Yi6J$\keƺr]5H]șaXC,)\Es=Z\Avt{E/$o2HP$2nM/PTuB8!r)Bs aYB*w3ȯ@ax:˱ŊZ{RBg\V.ΘL18*ƨ@HZ\&葁y]{R+&nUs(d!2+zJ2FddVdAjC2j@!%gf4EC~J+͢Yg%!{jڝa}@jKP; %1=j9Y%}ۊUP,j~D$qX3ٗ{֌øk5nl6^b=a^T PFlͶ}Kܼ% fSVP+)YfJ!CGf^T[;IfD&jtmX&]id/Aovakp.F 7}[m]d ΚK!F2+L0$-)Q!ar(޸pBխ4 Y͒+YQȼ2 bնBk%\*AX@qm$(/U@ɀbԠff٠s[Z@NJy EU{MrW<0ؾ7 bH,w R0v j6ͭz wЏOc/Z)S~j@qIP;"S0d:Em@r%jpN^t/3UcWk9Qsi2;2cUd%/d4Fv6dyTI'*_zׁ^pMq/J21YvUH7\%iN;)NSbnYֲC rc d"Y ] /pf@|qMݱyLy8/ ٍQ uI)Ц3 ;⋘TtĎP:֛41%1Őkv玚Ø^+Rtfؾ(GgDhܸKʣsͶ~mV`UclgU(ͮgЪ1(833S$2ߘ<FbSWa+VJf7i\$Մ/Ʒ׬[h٤ kuɶDtO(wU/ھj{\" eڽ ts-IJXҟ}ZȺMt]X UXBCԅ*:yCjZ8!ՀKZGJן('3p'5Y%D 6]b̺_6`|h@K}zx]~6qSzA4`hT z0 t![yneE3oqj%"9 kclhXNlنst p[zۼ_L٨M٥;ݡ/:{A6M[.f2!T"6MSk)бHߢd#JÓ축d2|u1MmkCb$JB=d%é:YqܵQx d0^HAVƸwVwl2*м%7PmnSLHha ~٦e׻iw:W_MRY9,dGڵ\uqh wW=y0[ŀgQsƀ^M\lL,HƮς{g~!慊MryC#2SEVQ  cH-J8 0D)؅/ng@J"EЍP"O\3rI#Nlx*9MS=iKj!6) &DOlh'~xg#6kV."b_n74VAG]]ku6lkݷd6KrnIB1TjQ JܱEJF雿 QȄk&'y(d{1žXB0UXr lط^W4}KC=3]D5\Q9!`,&7,X\YEr1D\!c/Dssu.JkQ?BΩ(c+ǑZ[)Fq$4ߪt.,k0FN$mHRAw0N)uI]5t9k97~ތ{f0+*\Ec,!M ;{]'oI(\I(h!Ͻ &XXxErh9^e˖Akb! ͒^? C[ύ~P Y.#!՜[2j_qK@Ʈ>|IlR^s7Tǂ[WVbR~! {%)BUl&[Vƻ,:nfQp1O(\4QwHZbr}7XK3חЇײ C.hNM6K %tGb]nG:iKB{Y^ۘ MlN0\v@ 2tRRTA&u7Up_p[R}X%ʒxz09U U {B˽m::[Jʷ~n|]Y1QTa(ֈ ly}5(-w ۗs؉7Z5uvGܢyvp}5ͳpG ؗQzF1X𺻭k>#;c6'1ц sɪ䞰WTqw4{ݡtw?ݑXHB!Q(uva#iWYGG 25>W !i*(4YkuqĕqV".Dž9siz.ܬp&h[(I%ۊk+` SfCy;Ѡ Z`m'-Y>U!7ӉXCwyg5_v_ޯ<ؽ6zě:İy,0B"/b@My$2$ϖ~_K,~fଥ_jk JJcY+Ϥ5bh-‚pb!Ӕ}&b 2N@d*\LpUtyJW\#rbջYG;4E#!rpBe]2ѹVj]t.]`w> C2,>ܽY=3LNPL7i3ܘ#لI]y+f&u$ W]ΊM<mn紹hegbiMxC V4g\ǫ楂Uh[l:7*j6=b͆_ Y6n V$.}4[lZsθZ@J;1=XډhܼY]3 M0BڵY)Uj]?smCDVYT!FcNa.s^ 8Dw WB̻4y!(P%9nF t<Ԍ(͹Q,z v$i=@۝Zgmn8:kq-h"0[1WMc/nM$wːa?H"M+÷EMAէV_rX-Iݺ9ٜ@AY8(FWid >V#`Su}=YzQ3x=c^1ҍrB0wq!.Bܚ]gM G}.t@R,:T ~TnۭEZģ =ބPG4Txu\ u ]RjE!ֆl*g%qJ m;PzECͅqW0Up! *7(83XA$C7,̻g⤳.J@pg׻LaE&U.},/0"6U^(Ԗ"O+0L!+Bݲޒ^LRp uè$Z.frڐEG3p?>8īmV$(8PH\ַf-VE$Aѻry(WܰOU]> 400WbKO6CCAd cVhvH\N 4;GО  >k#2QX4ׅ[yw5 u C]3 SB{Ƭ7LEꝮW ĶY_Jĥ/fdmA_\/BC\a<]U.Pn)%ֆME&l[!H68$.IhEwè!p\6 aRҕe\ՂWAipWXi~0S7EwF I]%2$lӯM빜% Y~GжkjKLWh'p'w8$VMa--%梜x*qS4Z]k540'|:ܨg1$!]dLyb!%zz%`MequLC#-I6 pMV'm *dg {nc-ʢրU [[Tn/av":#MBt+ԏ۳8%EvM*ADJtи$1oM$ -t6/ 2>w "YIY{Ei(lxZd㰸q#-R_ޒ[t4vG[;F@/0 0+U`й3ύ@z*wGNGXT_RuB 1S'Z~= 8.߇F:XB qp>7c|t1,D%3Vz ж;7v)͝W}px(jk|U,W-"(js=QW'u9iex;W^nqkI:7D LζfD!1r0IYW^0OʣP,}B'Tapi-v%$*tTeb{E;k[IZk,ZnXD[XpD Ԇ%m9%i^w) CӯwnAܞJ^X ܐp>Z]hB)L^0 Xm' lTmxUI8'd\,+brbտiӿ\ [$CJ@ iX9;blЯ4H8^TD9g4D&dh݋OK6-öƒ;l [jG=V޽Ip[V>!٨bshDZEۭu_﹨|x:uVZaY^hsytps+PKCӥ7a9J\gbs7ER-FG_. 7YkH,W(Y STw娺t[h4^kh[XI¬G cn:iݗa,hq &7vqit+Ѱ嫂m)6ʶr&4lq{>USe;QHY?ȴkf؜%sYL/H]!4J2ky_zJJ/&WپTm{:XSOv4f#N7^jeaLbPVMaKvhXEC"k>N uMXCFCz7{  uj7B]4 {'AX[Ht%6|4./ەI>*a87yfF^RVvԱL4NHUblHʔiK$0Wˋc_zqms+Pޚ jvi8#gMIL{C|Ff.}==;xXi2ܶYaޢUH;r̫~ F'Hi- e>.I b[ը/!ې/bO'# RW JQԺm带OH/ B2J@b"$qLS̞-w>Wa* ,aXnh<D0=Bx"00svܵWkHJ"E5m.nFƯ;2Co9 $9C[Fn.6߯yb0n$ph"DDXM!`C2y1fQv#;n; Ho|M]Xfyڤ^m[Qz qOr4+Ob^.vCZwTȆ u6yèm!Ƅr;?5<!LB6mP!0D5fC`u OYNcw*EJYqZ׀mzP#$ 2`[!fe|vWgh6VeZO/BWّ8]xyuKx!2XnVu"bPu:R۸!Dp: b!2[n97"Yppѳ*pP aF%LCFWM _(UTжa60B`Jy|Eds!u^ WЮu1w(aF4·-ctm9:Ȍ:\X 4(35t^` 8..U\a"V<;\e Pя {փщ"T6慌.Pӹ޹b' .μ +1 Y 5*#!4jcdKrY=.ij݀Els{aLsxL$I4j!]k딫(5[7]1\kqͶ$2"3s$MV&3}wYc! f?BW倇 C_ "IÍ>93# ިOU 6ٯžW_]Y %bA.ʒحxݶȸ6t&p荼d\X,r;;^ µi1f7tSR'@鶴nĵz%B*=DIb+5b}3$zƼUjnwvk%LZ-Z4]O.hD_rzєVYͻafn2/Đ|fcgfK 7ɥnGNڪ01KU 60 rwYKL䇵kXWiRw\I]9I;q%YWFKW{KnڎݍEB_758= .r[rJf,;\YRv7;%.C,YD@0-frh:nZuMXrw'[txYah_ v3Ӷ #]) lR0WT0H, ;fF}^Dy7guU9 XX4!#oϋN+MD@ ~!&:j"3 I?|FC([z/w v^(779xY4CΜ߯eX"SbC=9-C7NՓ\$Mҁbk ^eʎurS4U^gf.KW,ߘ+fn,iT~*p?],B!07)kU9?i%+nSp]p_eHε*;sE.d 7xȤkXIz%3wc1;R6P6!jkOk0S1 >6 _I(ˊ]}1;$ӀݶՖ[ԠL"!gy7 GXaHX˹:b.#Yn8&xk316ԩ@/,! mh_`O4"yI]m󰕮=RH@ͽó*@zRkVmCƞ̋MI/|F*@l2 \B"A­SJ'/=轠$AAxCN]" 7B zcl&pVjk6&( $>uenFI.6%NuN7O]ށTMpEũ;nfOmg]Y4[ne&@gCFд5hH+\4r"mbTs|vɤ )4V5Cے].YA(%:!Cvٶ_W* 6mZmH &0ntNM2 &b+vi3LLQ6!`gl{|Pn :D8|cޢѤrs6}rDq@pm.ט֯]N w"(TOʹzM V!֖ nٍ6+.iakJ+4!1Æhh̦UJsz_?Y(07l-IY*Ա묽=]J"p':h:uA_vj,uI In٨#>avIa7ئlvb/:뚷?FޝX1V~vM}lUUIJAg7A=?|+:pBn ?|Vx+B]by#\؝$f69dryˣqɐ=69Ci|UY$'+ΖܐMgi #$JYΘ٨{.چX6Ԑ:5I«L7OܬuTf7CKYNJ$iv}b-R-X1'\_m&2(z+ѿYV#{8'B7x3JF[tFm:jve7lZa)%umMde{q_wԁY!"˲*t-F4Vfd@;E sPFK5.2\q~Ck 4["y@cؕAWn$|HnĢ5ҭGI<6Ⱥhn\ޗ~! H4?WiCV!º7pYB#aF@ }":/ nlQTmyĤDukk|^McѤ7\͖YUzkiMΦ1u9K$"oWJĐ8q#=oS7@=! vҕ~Y-ŀ!zRtQ`$@10_$3h#))s;.6D5,OrW+t8J7:cB_tښK2EW !V\s$EÆk"f/:B{# 4.|Ϊݣv|YREfO:4&5+I/|6Cptu.&e@CJt>%nXD61ۡ r"5XHA j8b@UR dۅSt}v o!%!aw [޺8[+ɸȃY:tNQD;"ܰ&1Fԅ x.|`_\~MMk~3l%y(Adf=+l.Xj'E2ք8aD%Ȁ).Ve9 qĮQ2Y7@uc'Ph4-a`nD; zEGij :yVt:uYcك$ ñ}[&>K;Jv[ȝBW:]^(zn6/D^w6 /I>Wkp!À6聩,"pbw%փ_,x@UB=1V첳2un+˗ 7[%ʣM ѓaY)Ӛ4u48:)sSgQp} lZKuJ?9bU4QDS 62zMf]6I]8$ѪSݺI"JG5zf+!v*0T#Ez[4QrW 4ݽ emxbpYC&EdJ;ݧozI]@:sB?GPQ;eFUiSHW&3x$xL4vˇQ_]ѽ>43H ZBKuv7 Ձ+ bگE{rBz!뉊w&~ŦJ ,kȀa)P8&+2?=}R 8dBa B1knKC**RIV ^;lPqkw]p5 -߄%sIa7mݥ4c%O 3nU5bf~Fۆ\?jܬ*-5u]]n5,zA$jqoMmw"P Z$5{!1DOklH Ȼu\4tذ{ǿ ӖwvH/khέa57HZ]@IHu҉=h[Ѫx6 6G5FJUb a%' %+F3$aepS+uGFtA6$7/WM ȼx)iB?mQWv>UЅmSWAW0N~=Ȓ>VbQEޮ]%/ 5&d~37|iF9.U[8taI2"Sa9k&0y3|"w1+aȪ@/7>a@Ҋ{ jëer V`BwѶKS`6fK+Q3n@ӻݯCi/JO^Wg R"|H!ܜ2uţ7jA_f h:1dVL?YW>hV3HV)YQcQ%H%0tps,!G\\m%歛\V]J5n6,ko+Nrؐlj5sԫX~ڑώ22_g&VmӉZ@5Mm{4Ջ&޹ [D&Eb2C0r qDX2uL 3`PkaA^zsFOOyiβM궱#7XqǭU.zm @i3/]$flfNn)}bwgmKH8 }Ϲa 0M2iԭu  +!B"\pS6`r."^@ܵ(7]]1.wr B!9ٖ$Uh݂%ຌߒn W#!Cl {rE Y&뻞n9Vys13s2}jd#i"o%8ڨmQ&e'k^KJYDCn j:nEۂ ΛE<:8 4Xߐ0cՆ ȸ@-v D,&0W HLxCtLainOs4B:DO#2Dq2%2CL]*=͈Lb-)w6 P*1P5' _zuq?߆Q/[z0XX QU:ͪGT`.b2_G 3b`bȌztt~NZBt΋p/,/bym,ʫB(%2oa)T/4@% ZN/'tj%RdCdT#L _#2)8l!uF%RD %.C!qኒ7 k"YPlSg I\AhdM%U#eTJ'jh)Ua1PE\pl+wnrVk-/ YBlλѼ&S/(;IV>yBjLRЋ5Яh 7ߡZjR 5+ߘW4mvC pB#R\ f%``ݦTuU׳CH4lofh j]ɼJdXWʈ2v_6.6TٝPFw煑\] h dڬ?θ2oMv>\s"%v+gmХ.sH8f0نEK(g*bnp^H :W A@(5zzkm)!|4J8+Id M s4<úw~=ע/nPPv {:2S鷡Ƌu\@%Q o1" X_YÚ5*:d>CV "cCdۺ~w @JаwIbRͤ'Z05/*/%ox9:67qnFtwʇI0/n3o+uFV H,wQAG$Xi]!ڈ.,@ +=bfa)na_$ްp KrPwkA&pj7ji95ZHVV,WI]U\hBs. zOdo;:܂1 Mm^DBֆI:d =ZC2[%dezUQ$C]샬VhjBFdJ/Vj-[@f.֖yEǨ*Vl]NDlv]4@W2 Pۨk K:Et-SBM mgH1j0YY:H0+ZF|NxSt@YҒޤظqe (%"#lDіe1wS1֌0u%ѦTY+hYB~iay+3*mrp5j}ɡѯ;lBEBK.g%n0IDAT6a[6,7 WK5=VaI lo'52 >;ƈf%D@dB:<GUykۉD* rxv]g1+7nU[@Tbqzt5o_J#i*}n`뮔rWH J3l-_.DZ!uDElOIENDB`PK !pFuuppt/media/image3.pngPNG  IHDR!J5*sRGB pHYs  ~IDATx^gmGq_`E$8cc lr@$!HD9Il 0`Lt(wwvcuu1Z{gg?㠃{(?|:~{ҍGιOy!_ڇ>^>m~M;_rͧ/y(~؍$0[S={򧾦=:6{|Kd98Lv0LnOK7Aj@ULib4tKHJtK k ۄ: /qټ:Tc63u{F 8T0{/~=q6EÕ3źE=UaX 51i9n) Gڧ3= 7ȵ'߸~s|ߘd2_I !'îRlF G`f]ynl \YI?n:i1]-v:=h*o 2.N:ûG[ ΚlRN(xbeW|mNJ3<;4I}'}KQJC:vJrAIs]Env檳9Gu5i`n_4ׁ޺dA s8N32tK\"egnt{YZmgsΒ )T`BYrUC{W m3=6]}o:p,]mJ_Vr|'@d"UmB.|P4Uֳw E-G]N\f3~SDe;6 Q1/Y)bKXPxw:xӘ1LG|S.EMþsJ6/d&)%/T >bTvЂL^\ca PE*dD[O_Gι.d=J\Cj/߃N%k@&9w<Ghs]5'3WY=G)P`T:~>$&ύȒDR uOr)b.3P{9+Bg ʖɩ?ΡRNX?G6Hprrx]&V_\/q81 ю| Sd=yJ*0 b&}ht@O]sSM)\u RYE2W6bYb-@]sȧ/Xy/ʀchG &I~pGh3;jπ\.i5{f(fCKY5}Y3VSiw%  XC[ w\(5)zyu$:qy,݌#D`>v\;9O ZV ˥h5OAwGoHzAHR 8'mL\-N>b3"tXIKBMdqxm%рD>mc. !G9nL2a9jBGҗ},F29 Idu\+xhιn!~Eq_G~;yQhtDG~pؓ2 FjIG,S{ЌW `$$Xma,.r&:V`^ht)1H .q}Y9(أo6 bBl օRܙ X{ƍk]lSnVSvmE+ڣZN} /-ӹ]niRB5b]0] dI.$YJi^mG ׍݈\fZ@xKenӯ9qUCR-Ox n3VBjKE``l:}<o$ikm-$9oທsG ՒdtiUDnOH"^G|"Y@(DS7Y%XE(^V2+a2ENO$Bޔ8tE. -G:[Nu~._TH}:uvVL(%_0A9^8(WFM^ ^~QV ;vIwSqrekXTr_|) 2c5|GԖD"+tG: xƜXrnܲƢG봜f[:ԉL@1B]"=y{ia'Xzm,=[re=ۣݺeBW`InUsAUG:$BKZbߔr+2>*Y徭L 0_ 9<ĽP'+ȏ|A)uZ n'>uҐ˫'jg÷}TKf׹ )}N]!EUX5ji4/ٚw=:2lT`atU={HHϰt%߷$3e;a7'}ao.= WpG;=ס.7NuWkoFN$"嶪KT`&/So|NNm%> b(rS9wĭzXK`e/n6ƿ<QS/y uYƮm^+[w7m%qnLgpLw7Y>NxV?:;a4I7D%Nrю39+xqN]jwNrHMYHzH{(*~iZ{GBt::~В8k/НIN0 e~jK1[{d}&ZŒO^ț>XSBߤBfaD;4wZK.-RN4\f Ϡ2mp:77HuRI93:z"̗}&kbug8r|u*lr\Gߗv,KV`pAtMiE?3OI?&{I>e>ʅ.+T >sSM)\3D /\81^v.X(4gso~?)ĺϺ-0O?ԢM9y<ǕKܝձS"oNsqW,[ƪVF))sAأOm N AS4Sf1$.߯6 GWU9§Yl+ڲ"G9.i!ɜ uOmvZBهϾg OF>our]c=fvcñHv.A'~RoF˺HwlO=i3u&MUB3dW`D&$:Y%\P0r,LVf6\.. bMK*ʞ iۤ`ht1ȕ/)qh*)1U3ەrڧeqMzA%\5P{)Uz}@&D ɮڐ"ܚX $!f[.^(%*)܈O;9}w~˳n?d9/Kysbvb xmO}!ɧJ}4&+UQ1bG8heɦU,EMcAfa p]yojD3jtPfӱ\9OXNAt2.Y)3WD d6F\sul4:PVgb e̲= =- l50iP_gEOMRiyW򧖪jع bK.YDV]&WIoMa/xwwhPܑ͜[+E-[[bovB*dc3P*9ޗXtSnDs7245EE교0Iv`r# #sqC9uSW pbu|Sdqṭ릸osrF v'>Iʸ &\\ ͹zÔ.\mI( Qr2lc`!Gɽ ^DWZ{)Gx`_ɚjڛh$-W, qZF}LzPK &ԡ:]nsg!aT{ Î{neƳu3;%ױNk47#iu[6:J\g!:ՠp'*gbl#td&%ȴ:Nĺ`NW5\7<4q5&8iƾzThUF2gg6('3!MX62[`~9e/~G\t:ßy ^x(x$w~Mkwv䓍;¦}m|bxwk@Zۦo.wfO<wnVeN$\ā W}DžO fdf'z\.yVmVӱ> /iw~t;-۷aOY8#b /1.qҖ ÁD}l66bJR%y[[Hf`AP܍P=6ۛ9;oV1^t5;c\"IB_{V[&|̹B]X_&;/hOEݙƁo<߈sV#jXWذ..фqo{)<7u߭уoЩCXУ}1F-]3;e: {q rce!\l X7Dc +ԹОC^"%( u,R*0u( |nn}io~w`GUWUz0j 3'#|VTO}!v<=-G tv^vowl/nR:S:P%rC~b7qb7qԡ򜿽IpgT`6_PWidZxR~n߶C/p̈́!FU]֌=Df PN]aqj !jwݲ0Bݐ!ptW- ;qL4֮g(!M#BPǽ/uhBΎveu0_uwñʺɱtBݛ'eELxn:.ӕw5v=˽q=mB2UR/JIΩkR*RO篞t߭@(y_7cFZ0zk rRIM) AKH}M r[GDce΢ԑJC~pd."]+ Dǁ;PJ@tˣ'7u3(uvC\({-t# ,B)f}o_ڔ_ u,6XʖdH錛`=L{Ilg*eܧc,<4ENd-`i/o63ݭ^R[ /eBtqP8_`)fSv>o7Ih9rcSrh#7*o,W|Jʋro7UioxmfMt8^ٙvsn$:ˏ;g  -c%J 4@l(c{>Q g܃d^@t h$r9}pvr] !=~,m)Y,GـsLuBt lopd:Ƶr[[m+[m 8w6utҚᏼgWkľ?!1.դIrݻsVSĴ T-=ay.;Ug7 4F*]uu=\7Xg6tt{]D7Nktz+Mz/f`PǙuᮽ̉u'I1o.9̅?0Px|{Mt|-1|.6yn-|NLu溸bW9Ʉ:, l*0.k9\K}e˭arDhWu}W?Sg`P7 ;żdd5]*y`B c\M&'R xc]z-C&yɪqe:IW'gٕ+-̪^%Ej 3JJSe\W&"Lef#~Uy"2nsם;v8d,^Kv` OKw[_N܊Ÿub} .ͤ ޒ~}ʝv#М\\M]U{~Ā%cվu՟Rsb]59̓1߉= "lSѬNm=>.d3y?Lܝ)9@|s^ E+u- -b^:El#URj`[뚈q.|Dqj%KP")Q?Ȳ.v~>Ri ,% ,֠βOh'q[j/A?ߟ-%td'}'Ks=ekf xqD'>raTDYfT^.3GW%:WJjw,O{/9ˇŤ5E \/}iEfX7*)v\{YFAttL!:kr]EIK#A%=;nڠx$prq2ձ3VYNp]5;bйQM;tt9p=Ur\yF5Q!~pC ?)U5Oe8 ,'V}{A%{욜Luu/ĺ괺4nDg;^ۦl5h]%Jc)W]e)bկvk##9N#:]xSEK\/ \wm.߷}B<9cp>cKR3d&{[ikˉПqf!\ǭ5,c_m >U\zHTX l &dXIzn[)'sw#~s$-\W^%%4:̇%/VJ.7N̪\Wu(?;'.y}WI^[̗3BRlRݜtHbwXpD6LgaNs!&M5|'JW>)Ŕ >? = 4}=?rRR5Gm(09`"_v@$M☇8SĐ:ґ_ʗ=^w¤lY]=vE-!'_yMɋN@Mw<.n߿3PGoۅ$QiAr,zm[\U.=DGwK5^43]iPxkFjcR@e_|'9/X`i㑷P'uMK?_'ϵҘլ3te'!WmC:J.Ggܝph8-9nh7[ۙk˱P޹ $[Zu+3rD^@Cw\7KIKrNj{E{ceU{Sv&C tl og~v7S"\AwaVk53 /[v̬qm4/VLizuQ9Tð~ 1,&s,t7d|'/!:mGǧLޓii60DW`:[@t!//ķW`}7wx[TjY&-ySbPxJeřkuNj/%&I_3?댽)w8rlKur K@9[/ÉΒ (uoFX_4=&α&e6ĹPStVLdr&AȲ?<A{9ywg ihUc=ƚFE|p^D\Ǯfz6 Ȭ򧹾-@ݑŹ/)˂:VX$ȳik@a '74^PeF@,Vc:U#y{$l"<#Ŀ(\7Ś(Cĺ]2ESuFkdOuArC$XE`o7󿶲g IS96M,1O$:/[*|t2=^'Vɱ7\x6L_Fp3fi>[Fu?9s7uMd:u57Mz?O xuJ.QβJJM-UWIm]5a~K߿QM9/n11rq؈msHZV'+$ f3- %NIDS%vú8|ߣoZ!$Į,eXe;IUy4O/Sߖe}ϰayWNY~IGX*)MDgQ&Ib]3}\cNQj#qLkyOmD$\%!#@HzD; 5 /d?L\{GΞA<;pՇn`Ŧl[CT$dgUR &-3(aϩo='gDcvPlh [x,NEeQʙ=ͦcu ֝r*k竷26E*2f*{Y#ڱ/i ,}kPMrAR{y uHu}!ϋlli;RgfȡVBކN:Naq1Np -,J0X%l/5dp.]u\S~q;pb.xG%?(iqkQ o qq9e0)UXZ_jc$Eա7ߥ̃KhəDwfYL -R ضU(Ĺ't7Ju%,Ntst;Tֱ&qr\7钘Gn: 2u?#'sPW'AR{3odmzx`[%?A ~&zX-XgfWbԭpFF3}Zcw()ݮD xZ)ɄB[|UY+$9C[_fA bu-9u3ܐuMK47:u<]E@] qb]uKc]So~0ybRX.lpfVگ`蚆 wDQsO t䌢;/}9 b]JJ-4n"[a8ֽCn:;ѱ\Sp-u.Nn7B Չ8 -U.[ dkP/Ę$Y'aLJR 7C϶Ό+fNԶdFNM^% [ tXtA<-aĺeQ,|EuMDgBl\Jt.>W"\sj&֠  `J:5ߓRA-}魶 3D'wwIҦՄo`۬sNlhp:{誵lpW~h[A7Xu@WSxֲ,Jx4n%.7.LH|D`u®ZoYHcڎ MXLxa%#q^]%þۖЀW-+a0iÖtMw3ĺ&ml:[NOSjߟX^\mjc2p3Jv,9_Βf[X&ONQPrDG!wet!N=<A_r|t]xi,G<ƴ:w}bwAYQÜXgZ+0<(r]YVI1rMGtM!M7.CsDrnHez\t'sf`Pg?,%ncy@Lys~x&ٓ^ x5j_UKӡݐ0Q~e:ґL9me>VFnxU.#:}:3Z6^t #n`u[q"K,~+0%kຜdD7g=ħ O<&"Ip]!7R9Pj`N,GgkR*SƹreyRHN̺NNG8XWH<&Ze6?p29,73?{LMQorݤ|lu}dGn#) $^7yr*D8Cd'JtI,m{4elz<'wؔ(16I5uT6U. ~,έ*0Iså<&|ݐ u2&ێ4WHEq1ĺn2q<'s~3^蜙p ]Zy35ˍ0.$ << !w?bEOƞ<4Eebct8ڍ >!4ZQ^2GoX-Ktx. tz,!/ݔ]"ǸJJܖ8nW d.9nR_qO])Z RW3%Z=7PqO'lUR~qx;7KGE:|F?i[.Jb.%C}/K&b]/=/ta2&^OSrdgZ.@oy1Φor]W2MC-nCHA[pћì㶚>urvSlG<+_Tv>ExñK{+m)h'o]o鋡ڪ*fu t=A- @t˪g2Vn*otÙZ+p]p1蹴붂v+ "}Ե.bۇ_wޫI[vy)x(տﮇW}fu]) =-3t|R6NIi$[R) t LX uXWglp/}ۗw7Px#BwXM^Zpp ՘/}Q:2HpE'Pޤ2uŋ&zy2pж:لR/W>;?P|m(uxrG\IC@%DD2!*GUy|m-q&=Kݏ-b_Gouj~n`'-x9Ga+WӀTWr(.g%?Ϫq'傶,Ō/wV 4_@ uL|P^n:MPne| 4=^̌s&W+RMDmI{Gnkʶ=],޴`9!cDMMQayڻw$,ŏ2k*qePAu!e PGhu.wБ_:8:F'_BFM 3NGGh7>ۏ8K7o߻H&avdkwQ8UgvCGRQfkllmU~.gD2݁^nzKz'̸k+p=ڪ3, '9\j 'ڭhm2*:_ 54QC]3{LѶv~ s/htS5;BGֿ84(YbX /yؙuIcbf]A|(DIuqmL9rqN Ӊ@7ס9'ڭ\t3 Ä,^jg$VʐM07$)L4g> cxH]O`g:GT=.}A3~gaV &b]rŕV΄:V莑Tk/;9y@tM˥M21xݳR [;>uS]r;(W)n"]϶6u[z5A LucGzpNp?ud76wG=codٷL=[ZlW~⿭ ΋ej-;Cr^JRJ{]+.t ,r:/4gIG7ʭOFM2%m pb=n#XGT5ZC45R}VmBݐa$e쇌w9N΁f3㝔Z>=R&&kN.2Y&!w d\eRӑODw릞P#DP7P2ֳM.9e IWOavɮp]s[{Aole`P7"w{Ґl߫5r^MrbՃގv{jP%:czd:b8 f9kIwg֕'mKq]uNb:Ń[=\]+5]xX4qol5>jէzkz9Z0&0@ c%o{Z3PMEpZu~h7]lԝy|b!U*aēCFWuDW몫..|/7Vٳ;7Wr=z I'O˥8DGϭ\&D=B׉'eźdwn_Et^2]p[-sMunSrV[NmBݷ`lX+Ev.`Z ϺXk2\RQȉIn'b*)w`hul wZك!* m} D-ȡQ9qtdW}hpo /Mq.ItUR\o~1 /O xx]vn_%.Yn|Dה$ѽWp)d: \W.l9Lt Y=%9swS]Mt5^3 l假Mun [U\H ).qBRj \KehFIp;w74lVF\l_^7pHfN{}.'QFNpu'lHbź: UYLzpbW&*un\4.PcOe :u꽷 . #tuoF~iP5ivſ06?oÜ9q7\GM:cM2TZ:Dǜ|p]45xXb+:.e팱_^$jINKs,LNr,{)fLޛ:ǞzR{iFVp=]X{^\?-}Mu'uJ@Vj\Buzj/댉Vg|)8ĉqPŌ\cG]"ᒝa,3 {!Q%Ѯ5It}LJsCN6)v>kHlY+c$׭S[̟-Cݿ|5\+Řr#eլp5`DYXW^%PΕ~U tMY6E2q*;e=6-D3VxlLg,DǫP(,]uXz2ҝT`ʄ=Q[u9p'AEj?z4jlUp*U,![:IA,֕3gdg!v+ϲ"!G;ثfA[:&oڸdz\ew\.6Qz %+d)X%gi':zХVV4:^}0tǀǖ\VOs]/ASeLG߆/BQ\',O#'=`aÕyTuVB6[׍${Ax1LHݍ}7FX5#վ&xT>g1%_r9Q:Vsźdz6㱅3Eb]\x9{\@@/4wr,}K֥/ޮ7c un [F^L{nT:˴:TVgs[߃#'&tld9f ՕC Riih#\+qћŬD놈uABw2-GM[Y.qt@7'%(D JN^;U]kg`P07~!ao$a:P\nkJҞߺ y:NK^]P^&3]\V-6hmO[WrNCOmV w;4VZ2;ju˪CVVh%~Q[̚ /-c@OסEWk=mB].}zFd:yQ\gQ= 8YJb|Pђm (7AI1d48{Iq% ;xA+ |8Xpux-;,JAۗ Km_BwS0^\Jtrsept.}S 2Kc*bܩ[ lP)X梸no:h-qMb]pd:@s`)uϗ(vX:r!JJcn~ݺJ6Sd`)P09ԸR帮ip[SnRoI' B,0Y  r5 Օ}h&xa-7590T<*<Ю_nXgR{)^Y7 rK˧ŭϦ\x),\bΕ:+_pCb\',,uS]U86.K:6uDfk56IƇ6Fg+Ϥ)r864U -wcM+}5so4}Us=DFp71\# WŕRp]_J'o13|;YB*W]ok/\bEM9$ȡ(KGz-]u;[.VurQr6/|ck?vt>-Dh_HV0XgyM\%ٝ&Ryk4tFށv|MceLw+)n N:.e9d>|eeeA]AXGNf\7:mUoqD:&$jfK$IGxj̱\GN5gu{wWXoqGt :eQ8{e0v?Dl&=_,7LUn(}Fit(tqon3cI|:lD"䯋KJt肁O!7dtfX`Y=NЀvIH \ϞK{Y5쒝4˽1W>-TKsHvβAιp]n٣]- ̜C˷7Yuc3k|2M%OMtD9dB6:ٟZ1s苤*Il ̗]. Y2r]09.ƹˎoMgD+*CNCs]1'+0.ӵ]2]Aݥf$X[ABKR.W>cV'iCA&J.Qrtz4ڂ>" v%Q{) sƾH(A'z2=Z29d:vǽͪXݮFj rÉPqVEAy]\{|1JT εo)9iɎrzN:+z\Zu ̥%X -6eOs*ChF[Y%swuӥѲ=bysb댹{w~ug3^r)Ν ubuu:3pE㏔"G~zJ.(Z׺l+Ϛ&9NNC%Ol_{N,sWz@O}A\iVʐ|N&z(Zq[eCcM(UU(F5[M<*cL4&UIN @t-yXk SZ۞pHn^g9$3tw<4A CpgX^h6c'ԹSaj0F,NdSy~{ꘇnk`uDu5# ֱWu&՘It|2oXc, s~$NL'_ӑ %`R̉,u.xwj ,7D(:7!ya,鰩o0y){Z\Yl$&ިAHA{$sF/K# ,$ŐkxA (v՚L!~+t ,\ |'׍%:nPNV.aR2 uKG%( ΑѮF t],\k/n bn:c&q>qN@.rNìtIt=iq.hɜNbY=e`WWkX-oǝ`  n 2$g~=‰.^ kLt;OVHB.;A\Rcep ;:v3tt5rOc]<_heQnq,ng_M/ogt 7Hv%!, W53 ~2Bjw[rRI6Hd[2K&4O-DHBR;2 nauA|Ƕ^MU8ڣߔ}1o=hS*D* p#7rae{4Go<妎3u98;啇y8[!~Vk2$~Įk&AAr:O 5 L@:?SqM{θfڔ~6 S폭|,sѤw^\K Kښj> G^Zo>1IK^(drbӦTzt/cy5\T9l4,07=$;_z@p*=鎌=_O,i_(Eaq9N߼o^VC-eY x,|]RGC/Os}QFs?R+U2p]x /5%~qU;ޱ}X+OZ7/ }5U&9/(uIvO@=9vCܩF8rŢ\l-f`P@JS4݁P )d/JǏe@])6.d7] ;Fu9/<7V f^4ȉ:t/qS@]0Rc$ IsJ%ƁWP箷alLu=Po"iGt-P@kxx uh,4*u1u~0r:4uu/PItr28 PGuM]9p [NXi?_ u xI{\.N38lo@ v וW7h1'{2sT溜IkRN"YIoF.ta:A~YN 1ޒI0 t&D&zKݣ!|2# upHscV$IçUrDFC25J]?-.sxDINP'4xSPG#ɑ:mS4YNPS^HuH J߿wRZr) :Lk_5Бd# NN眯u>o}'K K&w:u_\x' woqA]?;7Mth;Ms]RǼ/VI!:&Pg':4A>\*!Pw,\M \vCFNd1u֎P. ,쫉s 4uIcG bD<@傆+USl% @Dw5QܰN7v-m%NsM9d ܕ}rKmiS]"ks]7oEݜɛ2Lr}Q /|t/6ݾ-_R^uc.0Ƴ34mt~&991_rќp>l&Sr嘺R8+[S>~Ir7)@21lpoK:'DsP4)3^?ѱ;uC:: ؆OZ xx(YP[΢!u(P9^2iHt06B]PxS))ik98Hs:e[[bVuHJ!bC?"Lx2QuՇ•vb\LVQFvjQHB.96Iz^igȐUU{ݛ M RЂvU0::@W9:o:!`<|ί^OEn(W8<64ν6z}F+, ooFwB)Lbrb]RcX$@]U1D9uT:#];B]+ѕ8ehtB]v3 .X%)֡R :;x;:y&/pa]J}ecx_xd/+ualbF[.H,z6K?r0B*]ԊsVk"&:7.G:U=_O#%&ԹqSC}#Gr#Yg 2Ci,R୬iqbCV0J*uP PWZe#V;L~`V2 y\'P3:-m31s7J-hg9mrOSe#bx/-d qަd&mioֹA%߷i!&%sMtuƒKIcGPwbgNcJ9fuBtc2Po>.Po\$: JbVD愺L','TVRugDVPWxI+Hs[:6'ڭDWl1 :ug&^PT`"Ϋ·AYM{Sx歌4@;Ñ%#ɫN(duD˨)sv]'IInLJtIMqs7iO7 e:0u4Gwu\%&+B:8,Ǐ@t ie u1,D٠2/;yߋ׹*0^uFV"L].o}!X>uڐX4Eֽ >%L{:khñ}RSb_e%;8ˏ)(BtM1*\xѼ]rrbJ9s`Cy:'|:ou]ksDř{}.&:LW%:vrruU+tڏ i~AKl":tIGtv8G3ƃa9#;YNp ?uVΞr[Pw<\@V}< 'owÜ7; 4*Հpm+*Yl05rvaӞۤ%?: t8\g_AL=^4ѝpˋs ~+\7D*q%wW"b8.Y{w-O{솳ǧnVX::@>cX[+@;2?B:d{H08.it}H:Ʀ, kLVLI&v*S8s2U |_NțIfqcl_]]h,'3"f:@nl j_%:zpL넺XPǨyKz5ͩ]Rù`Bju [P괇)eNd:3ԕnc)ޚ@>9uI.S:=Jt:KeyrT u_0@5 3y +~xfe.Y{)S +Yj ,TeX^y gxxSoM_},FNA:xl/ kb 7'0]eLKnp<'pk;|!%Unt%:{\@;L{fqURx?85QAp.ItA1 L״Jt=wv ,t:0 2bzb 0?;H\׏v<Hs"P'`G~6I\ƓVH S}or2c.w:ڵs@9q8׍ʥݧ],It^\' `&VFbWDɥ>@9Bt^uWڦG(%q. -`/RJtwj ,09ioqrr ĺ8udg8rWvt?wJs=ۉnٚ7}|AuK&О2M*Yۃܖp[N!։@qRkZ/U BeNkMb)fo..H\Itƅ. b]uX%w2[oc 8Љ[5eae:ܣ_p!Bw=D7jY-˿ O.w`aPf5XZ uL%5:ẠL Ltb w?gYuڠRzw$۶ܔseCs0L1;ž+;|t=m ziy[2|Dg^2]ʥ\7jk~ò(WFq]\{%bWY)=:w!} D(i Nw>B˥X{In$~Vj5X`uZ)>ut%+Gu1Nr@sW۟iW1KnBb0ōPwZ3`E(c~E얭՘n N+u\&" !:k$;HO4ŕC}96xf9Ktl,{lts ݉|OTآ{D-@tkڂs+ѹp!X:!MVt"Lr]0u&~$Y֯Z/ ~4 sk*/cp2 ]ܻ#IWvܠ|:nu1:G1ykeI]!N3]n-N+_ ΁Ǘ/K{3L$MOx5E7!i cI%X]5_ a nI[ EC]UX4ELKV`Ҧ%/rtGx4t=jn&9bV*k`H> vWS{xrrh7 b]ek*SlXruFJt u@^qNZݟrt$i#MAtzKHkhooyp׸ ?}NKssBtA8gaVf`Pd 8ǽYܭΞhpeci˙u<-ŷ=:1wm,<, Qz9H,9,큟`8Vt\Vݤghq3.nٙ\sGeU׍" )J.srpDENr,zK_`lTQy&WeNNԻ{b8x`B9iBce4x+ 3WW[:sU:8Eq;,\wg`p6 P!5z{}g87gHƁ4Ai8gبi͌e}ރ9kXSo uLZr ]u2 8e:T`B+GdBxue`}K%x|*8-yWZRY8jt[hm [Bn~|k?KH&$Ĺ; aKwo6X39wSɎJݗ?#p uOW? 71rWLAؗѡ&ci[c}Kcf9&8fvM73M.6wg,TuR57>zs9{} B /`v-n-MvWVÉ w4u9؟,K4ͩ <<֗obL :SZr#i-;y_8\4^bKq$;f uG^۶.ԕN+/zg\-,Η\rk{qݻ_nj%ќM)bkZ pA]x)+N;_3693t.}k- Lu9ځGo!:pNFsߊ#|h3rfϵ[9"yv]kG|zz :^ >W9n`e. ^gŦs~.EUYA>8~ZzlMg9MLWAt6=ϩ1NßycK@>},x%H^5-nyJU\fwe`P'لX\"U.¤t hkwu@;)hJ]LweGd>퍂x~yi[)H:&@FJZf D 0s@B YrInz\F.t3bE2N2(%-%fm#~R80˴t˥-.q\}ˮWN㺙5:✃s$:Q>%ger+εޖ.B&k#q.*q& sҍ]v+ 9uzlOg(L܄:[::M:)֎Ga:t@r8U{ 7C%^UǞWkB#uYE xK W|h覾f:D+\'P׿e2{NN:a[[,P'ftW :Y%s1 uxuԕePW%:0:7[.LBlb Vjt1U.XEO+(u\2xP\RmMt+Ε3 &5o/\G/8/rP_u:h:NhGp3xʷ{n[{7ZfVX[l'{ԪoيsN#Ν-o>yc!iٴ~(o؝6\a. uiNw#::Keu%RJ]'5tC+ 릈dJth@]$@M8J8ŅTr ѭ8W 9u:@t...təuxK^}&uJ`POx|gSf;o}؝F'm+h":wzo&ΕWyB+ oوN5Q@NNO 'l+atQ8gqBp?cβg̺*ѡ3x]y%1-Dx]"c%.MJ.9q='nRn;]X= :u0ˉuZc^1OJ1er0 u_uxe:LWNC$%x=<~:گ^[[pYWXgΫ Ğ f-Pr\Qb㪱IUsxƂ(V`0@']7)u(J=9RD3smY˲d'JEORVXg8]q>+^j/RL*uvRG l. ]rCNJ.F8's``f]3u:\c)˥M*çuxMKr'a $^Yv[]YpՏNRg?3Z,]Ruc3j۪A9oN}8uĹY[^L+6xPgi9.:8 $ Mw0q.<7ˮꪵ&r],'Nup.[a:˗6:k9:K.iJ'ecJ>]xYW* paz:: u,ЗvsU{Ⱦ=ͯ^5-o+y\vG0:u4鄺FmI'lyE:ܶ3~h{_OqAU u:(Ɣu"p%/;<ײ=:(Eui:I0$:C>u:L'R73!g•YN\xc:RG玭D@O3x!:a9 tx= uA:|J.:;\nX^uAv;$U%;t99>kz̭~9FcSzijd7[.x_$zX zFWhkwqkCЀW&i.NlP9lPu4ԵrɐbY׹Ny uD ĥ|[~-FNthգԡ9:#\MD'P*ٓ,in$:]8zK{W-pdfT`M.gK^HuI . uT'z^u>^̡~'oC?|rj+Hv1C-/\˗͍aqa6t+u=9Q K-Ift_U2^s:mfg:qN˜r8:݅ u{{:ikIیhߺ))v>#v::1'"ٵtl:4$׵^S:[MuZήԱ~2Itx?'ѕ={cܬVX  /-aAY`9^WН@2sr1βNfn1LA5&wS#,FS@t:7I<=NNɅ1s9+hw@wiV}ae-HfޑG*XCota&ɕ0˗tnjZ2}s;ϲAICNTZJeO!:G\}8GhIL%1v4S 'WŴ^s~$>ىp\xipg~0O^O;]HNhtPo}hyk\32ˠϿ-:tzcf i .A` RdstpAեnŭ42ƀ_ۂ \'ZyRsT/l;JѮ_P,7."-0Mt"}u'R);x3A$p^YnMhc^7lItlG~a8ˣ=Ʃ͈sr8wvv[_ް},z],ցc \ h'b]ҕ? V,gut"މϻ NsK~.㵦= qQ?q]-[L{;-46ݹGKf>u9trs_ԏYx)ݯq|@.S˃50!Gҝl3:u:,.?&SNϬV =nX\:OQ'B0(Լn&qY3N#qˮV6<쓩fٛ" M267鎶$׻%L1`_eZ]9*b! \}/Y.ǐՕ'8ݜRǐ:g}ҁ&NΩbI) uroBؤk !1w!mTkxxN]Y+,<. y4e!fT]j[ $A u.us0DXꍴ`v%mVǀb 'q@c+sڕD^uzz*cסw;3$ɧfu>1, d|gSS5A/NGM ~ב~_pN$>u߹:un&FCu1ԡ~CK.R22SZ.`9ѽ/?ܸ`,헼7 {mxDC~4r]5b(C]ѕD"P]PoCzK@@/_Ɋs꿂嚁zv1U:YSumױ,N?3N'b#cN[\ydW9V:i?wG|;wQZ_gڗܚ:u'4uZHijyXEh:-qP'6]+;oEm9u׺Gh0S:v:Ldu1rPskW:xq,Htxu2(]rcgP.,\@OU].YuY).>'DGCW~IW5AaT uR ?Iף~cGݕv1D. \0?ʩsq\u΋2Wj:wMUWUwҝB N3B]N ,@k\)# uItۅ:-9i"Ų@e+u{Q/x\GC \t225|&usG褕Suln;#ױR]\xG7696k1HMI " R^u\УOAn:c@N~$X:wd\3n\'\\K{oor0CTIbFҼW2ic_^'*i.QqOo~ԍrnK`Vj hdZʽXXNwm X#Թ8!\'>JF퇺*!L'A.&:8,(u쮉@ؖhrrEzNd:z:4ue\: ѱSu-46a<ޘP',X W/ |<9x\.)w}С89xAn_^]&eI@t:9# /@5f!OqY g'gnȠ(ڝ79H/XEZ :A h-j5gt9cٮe?{(d`:C6wcDo=mxѡ;Q- & Cm ^,yfɡm/mcv2ƽ # d&?ܾFNB}q te9.8p#HsZ;+5`{v7Abf[hY:q亸׽'Uюnveq],!ii.>GwIΕ~kƖ<[Ќ>Ӂt}# ZMcF[8ך%4K+9η~8𜿽)6M~1֒-gsF3tzd`2 ?@t/tpOν͟9oجo9 |mb[&ه, G'JtkZ3E&Xw;P.V ތ:6tpP:u#\W~uX@޻Nu2A;Uדtפו.:\0.X47_ Z ='8ހ8י79qIwIJ9-<[uNpbю8oFי 4o}on Ɏ:}:@..l #R.v `il=Itl^M^w-Dȯf\3`:-%0)\ ̜L LUE PD.<:t̀2td:kÏTZ:ב.RRtjC$<ݑ:j,sتrMb]rT'~.;w= c7p dŢQImrLbj{.͜I.h2@NN6% ?nsDWDտq&&jث5 R8`RJRQ}Н,vܰt\ǽ $&\+BLku\4%^.EdL:P@tbTP \i-D'ES }EN;lZ4E^ԍXgIXX-5&B~?I>V̰x{çvz~:e$1} wRD3y.%%=;Z)"QK@t#b^;& 8^~)R-$te.qsx嗺uS嗜AǪK=nٗKt50 2]uRW].8N_ ,b SJ*y?o``:Y#GtvW7i@^k5 8eoY9  PWY:4̀ЮxtW-\AW.|O}ox|NwuySnqZ·ݺJ $?S%r@ĦrF uh ky# ԡוsP\n~__C]|"^At@`[~Gm*$ixKB܊o1Àfwy࡟P'UZ@]]_~$Vw],\ r̺_,u2sr19.W U'q8->q&G 'g#'PgNuadfGsΑ89R`b^k頃[oݬуPe11tg9z\UӮ d/&݉9ͳ݂LѹGZYW|]aP|=[":Y{6& 'pi"kБ@t;yQg؛ItvZA['u^KRR@~"b/T`aR7"̂X:4 *0tu 5RIXÛdTy³7kuqHgzu9N,)#yPxtHt'] + ŏHhzԔB_(>X:$\Ed@J+˷,Um@."M ]Y4.̉u(΢ЯoYG1R +<΅ W= ʷtNCXWR"INu**0-JzωuMJ:ݛuGz̋ԝkJg_[V(>~r}.U.kg`AD Φ#h6y-V |>7ƴ@ /ItP'$.溦2B>PǶhW0mAsK1rYP\J׹UR\g:t*ѡaDP'5[Nd2ԁgq9:]x j/sxSֺdZrik\300{RR60Kp/T@nSruv3}9갡0NWtW:wНXoYȉH_WF?pr@:cAz"q Cq}%N u"@Te:.[e::Pg':4 :LXS9.9u0.(uۆԉ~]2-kfޙSODX{I+rAсr06ud)ve\4Ea|{MQ|G {$Op>ˣKNk":7ί+p]DQ3`lȹqk>s8їp'dMD7u>/yQ9_[{+ G,L=pN. D?)fFyuk3ה:d^@8XCep6.P0Dç"јɝt(JJ|$źRZ%:4,7+O 8%'^o˸VNz]O^@tuT$NɎJ]pizXeb]U 2.Lrɯ :1W: &ډR׊s !׃ }yфvMZxUk*׉Lg(X0}25Yjqe ,{5:]{IFϿΣ3^l؃J/y{و61\gzX.UYM\ɏ.(0+W?JuMD']XNA8IvR'ܒ]RlZź‰ve@+/R|bvأ)uC`jǨ.Z/<\ t :ZS]UKs2@ u|Ӯ׹Aÿo|ii|!:4緵i Sy 唖"Xu :x~ƛ6&FN"I*u93Q .MuZ VħF/}dسP+}ͯY1uv\'؞. :?\W:^K) .:S\n/:ot upu8ǀ}P' }0h"|P Uuu*U US:/tqpRki󾹡nS:8jJ#ס&L]|UI`u1I\Mf81yD ZNCs{P~xv*xP-\s|'~tPgyvS﩮4 wP0VVC[;8@]2ܢ/1.&[&+Y~W6kf^:Ifvb ALtx3:C\g:4a)&w5GAY\u(;r<qoYi |PGc9 RˤG':s]CU[%&l!\;rc~w~G4mH`G:|ԏvPx,: r"jZPHu1ԡpm-Ŭ\3Mu :ѝ͋xd:|7_ =^f+@] {y_^t„+[m&BEC2 2.CC? ˢDWW5:d9'(Gut:|T:|*z~' uU!wx?ؤ:-&uxh75iCv$=m}Vc,Z ѧ]A{I w nsr,~:D;ױp熺$, aqk %Jvvvuv*׵*u"бw:6r=΁P/>Eu#ٯ[B>_;PW^PGQNt^假4q*: .Wr6k=u,:)Exb]u9Q%:v:4q] @eodgEͲ:Q U.v˹Ұc9C?r]Ա$5A]d$:+ԡ&k:1Xuu#cN::,P';LGssNNgG; u>c ~ew PND0n% k uH^ۑsf\&>N0on~Muq&+N\H\G P&}uKvv}urn}+bkV 4Uky˫ :t7Hv24WMZ'ԁ[_oڔq]ցvPhW:;;#pD0\#:vQ^Cpe:6,p] : D:"D>#ѡOLw[>7=J]LtC U70tDS@t|a|2'ӡ@^Sñ] ~f`؛r NX6Pw(-SWN ?,{'&[/{n"CZ8+@,I< :{f9"ZźK\ tNs\,toE.wY j h[t(]!6!:BK^_!:iΣ^`2bwB΃\>ۿmNJ1 b]rgeQwӢMW 'u L"Rg)l&肉yѵwKv9 O UNBu6N3%AufFp]yP4Lg约V!7uFz t|e.?!:cr2ECJ1۫ٚEe`CN7Jux gbu|©7qpJ)fA3OKZ> ՘ L q8v+]XGxɴ8s C$,3G$8g'::k hZGu3Ls̯+Hs=Cnmk:@RuX5՘8uw^)ILr/V]՚g:Yt:~*\WYp]n8XwtrJ1E L ׵ޑX%^2uU]7Ucډu\e{.'aX3YvF+d{6#q\2wl, $v&,bY2NQuKsTdz,b]LgʴiURN^tiue:{KL=7C:#5t,D p[+f`nubP\9Nbâ}vt\`rCbZ0b:ruLl\uuS^ʲ(&פ\9!-\0!%ey ̠AN4*-~XOBKaV:Ж٤,aN:p]x)2N6ybsO@t? &>i5k:3p@1qRKUC_x亸Rz:ovueQ$ttZ33LĹt C]ylOgx|v<qV]=:HG ?B4$WA08sZ)7~{}S- 8USGhw`9yA 9~׏/ْ,+R\FvvgGR{Y,JrS1 @Z!G/ܕ0='^%ND탯H.h#:0a'qZorYp;3 Kx c0Ht亭Esg HCL㞙ud MDg9ZѮа*u]X+el-3  ?|hsINoKTV] X嗼$(~ \>+0qE L4V@T`嗰 *0.2*R`)L'(,ߑRYqӮԉO6)a"G ~]<~+ZU`R]iߢe:p03 :]~Y͞>SwWcNsW` j) xZXNtKgPYUterrP'̪Lrӝ 7.R/5:0-BtECS(4B](*0PW-tAdF_&Yk/2.PG2?C_/ұD'xjf`8@\:@Q^fx5%6eAnfCU^ ߠ&:ԡ¿:9|FÏ> {'&OǗ~QɁ:\,>4q_<7ѹF;B76-n'lrPWޗ VO7{/_&:xBlgNtAs],'WdCwLf uᵃP]$n7/ f c3&Jth8 1 U /bb@-n^3?XnD)᝜X:a\CM&ҞuV8kUOqI!*uFo:侘Zl">Sp]u::v<8lk:ꒅXHznPA Pgg9&':L'D,6"!:hÏ:kP<砎6ອCE t&:tXK!:tA ɞLWX~V3ap9b, POb+0Ÿ{Nj)֑ /}\v#)E.JC :n!p?{g&DOF0t=G釺T~ӹҀΡ%"C~bl8 w+uI1_5d:DүqD:B]+uZN[uKvP1Nk":4Pg':7,2:#+0Bb\gАJtz]uZx|  R(X%?A+ \h2]&^1D*){Ńv :p]?<}-p ud$:xƿ6pa7 R,t]SX;utT3At~8A\xpSo7yPi%%1:M-sDt\&\`ƥSrD)MhƲe.6;%=T /V9K+nNnx!e:IuHnouu_?ũ'УO hgl%ft*AKjqrA2brej\Gڋ@\'Cl= !:ht8qL{]蚮BX[NC٪.|[!:YҭY?}FpD$•xi!:5r]Cu#H4 xZ2].*tcl":tv Gj5%.%eҗW5WI:(Hvz:%Gt4reKw-8ERd~uuݶjLFpDY9;v G$ojAV¥LW:Lq.b &]8.)U/Ml"̗]xO;;9:IӎS#z(fq9Vnvf` Pƿ ɵIu}ue^c\g TuX$NuF{XWA(a:{D{cd7L9AY|}u]?paLZo)WUcCu8~'oޏNO,S\Akݷlc^ړZE8N_'9:9aɆqEc_j̲L'׵ue:׍~& /b2eyn@+;59.gVM]v"+\뫓t+ p2&^x\g#kyd]K]n 0p"(ճds{pR ] /nx.un~_uC䊗=uX'2:lDVN0B5fXotu;(|= /[ź㲋uKX75э萺q G~ StTiuDK.V"t8LgF:g:7IbӫX%BeYna<c)M}չ=,8l{ҍg)(vnLW%:Xg26D]a˦RFݨMMtMt؞'zVE 44K;{cѱr[ck_qڹU70 :鴟 &"ӑbo8:Ξrn u0yX:LN*0>'b L9ʳ{(.¤ %;=T`JC^T^)Y(Ur}DWl,q.a%BtiuN қD' ǹIzŷbvF[z8m 2]rj҃͟y6ק}h,;> DgP!O%T\褭SbeY3f`Wg`ZYҹTrMɮ3p F ֽre:f.ֹ- DgpOt<!ӵNc0Jv5 U[6(t LLCxF'q亱2 AN%'lgx1ٗg2YuO<ɈLNܓ͚ݛw8.^1:uyf> LHveug=jp*0OuuU=^VVgiNtՐ2]%:7$Z) tIt+%3%;!&oBEt@;@Zewt\=8:5^M% /j:5Xʣ@725H xt]kV5kvoVXv]uVŏbŏ4u(a\iuoyzѮLUs9n-b۞Zce'%ϠG>n5p!s~z,-2) 9#~FsM~n 'SpvU錁o89Ŭ0ΡѡX?БDyLK,t ڬX.}4Zvc$vRY溅uiiWR%ӹ.e:]9piD[GtɵRFU]btkVw_(>o{mU8s k,2q>zntGvMt]#%ot%]w_N/"LuM >\W nw~Mֱam5q_UζtXMtX&:GEΠC'rL$'6\m It LF']r8'r1u>Y wjVD3q]?%I5i2ݫO bQDW,T]"@;˕誷jf`od`: A#ɺ)Xr:LXrh:v-^Wbr:߄:CcfX&ᮗʪwœ0.oNFyK@D [0Dì5t j/rpDĻ\)Q\^ꌕ.^%euΦtN`FA%VnNL/Xjf`dO8<蠃&+0ځh50Aw:jQz %;~y/A d ,s0O ϾyHv Bޡ/pTf43O#+ul_p^ O{2pC#AsHmPg~IwpAίlքp ѽkklެb!s/DZ~{/WCdE5^YjFgDUB\a=&&Fν J'ԝpˋXj/EC:;_7'*F:vԶuىen~GN?Jl@W][3*):=AxB]Lt9.4ԁC?E/ٯtI[jf`d`.:I [.97\ve\G{gXxhz:;p]+1$r\vP'Jxͳ9_Ixs#\?_91{Iua x:[3Ɩ4# !:_L㴺C]u*]DW&*gr*UNzP \W\ހXkV}]+%5$2>#NDwXt+_ۮuX.}u8ו:$Nq:?h'z]`_:@S_w~povPV:7 )ߨHhÏ7|^Q{lO'P!PGP!MTЧp n-b%9]'%U%`K:#aDu/\W:ƜD'Gux?usHuZtSʅU.Yxi:7iW:.c .:ht-n^Gq]X.{`QħU6S>K tG u(:LrHx4:X:LcCƯhjgpʽ:/:::!:xu sv쉷ZV|Wy']Τy`)D{.8JU':L:\5& ŖAV˛丮LtmԪRE)t]uuMDIN[ xg؆7h7_ /YuwHtxA2oRtLE6\3K3B]98GuNjLr]NCXG>\G3ݳ>'o8m#7%`9qCs u :zNu#Z :!ɴ:]Taتs$:Mtorv3⌡m3.ħvuIKB]Rc/9G1%ХЗ:7PWX"9d:pD'd'PWX%k]VaJt߱$ ٸSk &ۗw N@>PINL1?{;O X9.! 5At:I?Hǹix3ǁBzpu6!Dk>Q+-uv8;xѱuV$x!uz5a ~?Zn`&9.]TÔsDb].LW+^D{(u@8vItM4 AU`=J ?%6u`: x#u~r\[6U0M(9NDt8q#8Qa90=ecD׿+a`lܰ)>Ε=We:4w8d:ٴ WAcoʘ6\&(_ܑjH+hthv^ N(iKxU~ Qwg u륹NufI ,&qyLu1l( @thE,{S(HNKGɮ9gp샪XCTj5{8+/uoxrm \O!}3JvUÚ(zx,'-^uF K!;窿Ul:KRD&\)8An<]:Cÿ\G@t|Dc-\fLWYs>ͯԻۼf8p]Ѝ:dW:8.v\4v^+E?nLǨZźUKt L5@;e96n/Ye:ު9AtpmhPʥ&ɏ\?u^{%;ѕ+00n:Ht|Ve:+ Y-2]0@kb!2댵@? %0 g6zKLW\'<.4bzKc>9}NRHtU.s~(9|~dq^ /o5[33B]%/P.6EB& u8b6|Ҧ ?u帹hJFWWoY7RxY(βVZ]>d]uvcvf*ur]2*'j/zH[ VZo)qd@W :\KF' A#ѭ+^ޖ=RqxOe"L|f=I.R \D;,$\3ic!:ki_.%עĺB]Hsb]LK/wCsiѱGױ >)/ӵ@+ uBeܝ\o9D]vRăYL6.V(m%s2`W:J_Y?r= UsoYR l݅]+t[ :rv*D'1cmf`Of`:e ^9?Cd40u)h#ut(t/=\Jb,DKiYy7;Ј=C L_eV{`Q2ܻz,O_٪,U@rPduƒd5Kc2]r/fpBwLgY%= $e:[VK. o>äLG[ /{~m P7c%Lr]uuh, <\-wZ;0eQ˥nnԆu-{6=]sL#8nӮ6?6k%:9XQ=8's?8ږ. E^*9u4([VߴگOXo)]Dq2q+JΞr-+u?N.tg8OrѰ՘UM,uɮ>WkʖCd:7#[L7RDgϧbQ.HtKlXׯ`,\gAה`B]R|{To/daKl 1[bK#: щL#keU^  XBffl׉;r~źt QWcR5?tMT`: KM7XN&. `tqr:N*0сü/^y3ͫDWʑ)֍%:+ຠ(j/%{"*o)fⲟ,D^حбdXóZ* tApl&ǀ: P0)9|jXP@t 5\ˍ x`=L\0K11Pt0K;HtkqK 5e~sS]u: tv'vOr-80,P_+,\ uyN^U#6Wr hy0m:r\3pe`1W\: $J:HɎ\(u4q>"ڵrI내Ƙ=u:6dsrc uA0,'P7,/G0c}KzDǎ\Sۼ~vt0uAeR:ۅw :XpfFg$xM$?e:J`\Ś5qVvWE+u#Hvuq׉^:L#D.Pd9x+!PaV& tfBsի׋ /\Gc/Zkf3ߕ:IDATI'ב6߸DWMP=YrEy} ׵B P^uM8ؚd:ؓp :jttn睼d7ԡ*S隠JtCPW(tq% uZI1 /שtE\\3f F1;.וb]u]PY:NnهSi8qIEYYrm䄏fPN@cu@ng![z7ukPGc6|\tB!ڽ+>WoO9֝PDt9Tzp:e71OZ @tϺuKMtHZ u7S6UWZ:hA;!t>C :{?<*PËk 0+ԍdr;}fh.p00iJp `\X=Es]bNHt:6 㜕Glb[:z:!:FV\9,[!. :䀺@t9pY1(uFC$:uc%pNnVC&:v~NgmPW%:4,C]rKR :G(W|_3pf`^ \Cg }Ûb㺸!6C%Nb#a:} tBtM]bMab]Ltd:,THs3u=It9L#+tӵt>]tk股qmLU'_p(N:].iSar#I'Bt.8 > 8?踌JtMn5^3pg`nwu.\ఈuhѮ,֡Uu7zpnD!-Pb]<@%ig=m.ډsUXls!b13+b]^̯u,$ ]T,htMce:!śoE+t*0[ź@73'YuiV7)d:wsEktFLtu/ItMXo)6#N,B  xh-.wXÛ DKj9+Mu:d$}\] LJu;Sz@C. I[L, دVL_x?Qr!bK$l:/2EFitVS!:EI[=g\$ y-G;SIrW5ktV~\GNHuH.^ bW)PÏ ׹sd!:X7Lq5uLG'vNNlu6]UKt\rl2]Bk X2pЪ[4栃+B/yd99_s:}& &ř,D~/h_u i25l&tŽ a,vtx(I鱴ۢ}DVFuK1|Xd:cDtl&ʱxIt0)nQ\f~0( >.V㵤ow}u'q+q dU S(W}$C٨?9Z+uU,^(蒣~)hNo=WHu]AUN5Q*GtRokt,F#:|'QϚ9+z u8u:֓u %.:4q>"9uF;cP1ى1$umA3B84Ǐ~ƽ.ǏyƽӝuDG+MH?BtxN褻25]s RC~MeL蘍}|Nj!K_((%r]ue#%Y!'pz3u mj3U8&zXr[6Y3fIj:8%:|. uh!\xqgtQZe:v15b8-d:!.@8؃[\M=vk{*5c'7xBw0qdP"p:#щ A 4If&:|/ȩu@t$; u9+KsAgW=*>t@]Ρ$vKjt@zM@ItUU3] (g`-!u᷿kbV0aS:&Tl\kaC_Եt6Z[Y.J&::IwesC]ikg7lv]7Ɯ u1ѡߤXqwnIs:pè\GNڹ uUa&%*U9F Oꤋ Ҩ8 @bc D'%hN}/Xuc%f`̀/+J&סSsʯ~rԱ(/Z:a>7Yz\g: t: Or:]h:! u>c(d:)iU\:i9PK-y="*ۃvKSQ؅$ԕe: 1$*e@S1@uKN+1cN.ҜDt2wx(Op8$%w,h%:Y2 }+UY pg`:wz^/ZKvzĺ?Fku\@;DǤعNxx_Ҕk:ѱ/|PuPW9BIl\Ywshc.:t7?A0~KTuZ.Gtu06 w1Ύs=GtHPW9.)7ODDEZ#ikĭiN<؉.^+@]aAݝ@{Mtq% .ZjpV̫5X?~:BuhlCa_4JM:vb]uDx랲8e#C0 `[<~:.:U:cd:a7|2ۊs9޼!m~( w% vZ2yg渮LtvN C+s]%HvwĹr8?Dm>MI? ,}DR< xWq;ѡStkeN^_3f?+C79Hr'Nu$}\:+x+d| W L2AkZG=!5#%DWFx,tp4I_:$Os]Y:}3;ẀnR+L+\g肮 \~v< ZsU%d :]~I  D> D'ѱ#)Jt{ot!XnHNuAaw{ȡyC?sabI̸S]hAhW溂@:!:B]!eZBzoU+qPW :]`Ix :ԱV3Bл$E1P%\T]pK8.}.!DYLDtiN2JtĹ'pzxӟ]:@t4p]NCB]dKs%Kf`lXnTg; Ä0u!M\ڑbHtj+iCNcsIk8iԽN/~h}8Atl>Nd: DW>'N.(_GMtPGv@o?ItY&cNs#2ຘqNk:@܇1DX]7^x{/{ƭtҜ*IրgĹM~5@ǎK]5ભ?X30uV:&@Lr(bߠ3XXcbk":xk u`9n@@w/DKl2U'1uuFX uo_; Ա;ZhW:;ѱӁ\o"yn9IoJs]uz\s tAikmQ Ǹ߄$\?xJ!\"N@: ANZtN.= **xy޼?>bj'/Jqԍx:uLHCIi1C8ː:ёI%NH]W ,ě:pRG:#;y us[gtu*.@:!u&CX\7ъԱB:Fs4t{ӹ__h }r<ѱN匎ѽs[/|+ftJm6y6cXxS`^z=<%?vؤ.ہF^Ǥ^taߞ]~m)nEH^uD*Nu&I󤎔x]y ?)&us9yMh"eHi^ugtw_DNpuyR_ Zx1).5 :KOZcVΤs]Gh dF"{!3:Bؤ.۱nױ.v"u@^7=c W'udtxX!u.FVuݡܸ:Nf\$c#kIΆ?rrExMA?dx݊ѱa^ u|FG&tu# ̐O|GaA2s?~-Α #7qREMBh^эtw(~νJUonw]wҒ^诫-77;hm&u&D2^aW>9#y)-4dw#{g:ѱMXxX7I.ILFIћ.^"u|JHR;!u+F9GuMty!뼌sb@)1 OX11•09 )8/u/HK>Ygw/8Yb2)#%:G #GnK]}FWdF ˫Bš)??S^t&L\߹|'/z #FGѵ;𳈹}Ϗ|"IS8O?mG:"c [i0:R%?Zݳ+WGoOQ(ݳGY'_):FGDN;$>NG2Lxt:t9䍔4)ϼ ).ϵbt.-Q5gj9G\cuFGtѓ-_ dtX&{ߍ{ Ƿkљ 6C'u›R}s/uI+u)buʝulvúٝօrXgӱarL:嘎5u:# .R7HFaJ2n\SRydN#RGK.fCDU'x- 0:gJpFw4??[@F0tԑ*u#[83Iͨt^fU{}?Щ]׵N)kQ91ґ:/# s1:=ӱ$k\at.vL'Gs y]wL^uaRwc;^גZ:'u +atw`.FGdt\ H{@nY?w߯bw-FFǻI:t_EZ^ u4oBu1{>; Ͽ|}/]~(v8I6:S$R2:$u8cPFsNH]ժ*:'.yHHLFrl$ģ4~霞 -3FFБ@K6Uѽ>~j?%F@6;܃T`j/&uRZR.?&^ǣMיxy]dc^ctdcw.vLh,O7*ב SH!\5>Vg1yёNʑG"0cFw]g%\ut˓:\ 2aw^:GJ!t[Fs$2:FǗ\kְ#I]9g(^GёNH, ћtX:zsEua];I]w]>=K:y ɛ7Kϙ[뼤jy~LruI]ѹB_(l~x\+?xt݌T'uSFp96)}?B1wx.utG]w@G1K.匎_}˯b{AJ6*6Bl==~EԮ;;x]#uSjg:a^GOAuJ֊_LJamJV2s_k˙ѵ/ [=ܲ)N9!rF7=#ѵt0:s+_u[l#8MǼlƑuu|X1P׾;,0nǼnzX2:R#u6~yʗݛwyqu?#.8i :Kx]Gh)c3w'܈s![:ѵ\-gFw }t޿Ǽ.*ay3ՠ5~V|GN* r9vZ.çTc1+dR7=c"uft&V&u'|谎&:Ů}h [n$uxl{p7u8c^^1:Ґ$u謯u816S1:HH3:%FFGzRyV.|LfpB.׆,@FdzNgt#k[RGot^9KY /kU?&Q|f">'FG *TѹvHQR k2ΣG #I>O| >z\::G 3LblRw2M~Ư?o$:導:1Zv2:ޘǥM :ft$AuLhx׵4T:Re: ?$# l/\ȞԭةݽYz^sѼNx4#:Gs49׵tI4[rdt+.ýGuqGt3tFt^t%~s#6X-x#uG:R+dY['A^bt*ZRGJbvHuc:$u,Pԅn%uB:Rgr9A`JӒ:ΉɌtu#2#×/9_[wѵ\}>;wMޗIH#(o67&u78hN.¤輎o2.XtFGct$@ѱ#:9ú5T:Ru$o^:k};A3BW"6~9yR2:sZ1:u8#kyBdZ^s U|N(tQӏd.c:A~yʷ|nHDlJM9>^rIoUftf! Iݍ{:uoίs? #RL= <#\ndt숗u4x]9#@^Izy\Hp9!r /^ǟ KBb ]_sbR CXú];NK>_x:9zጎԽˇ~]szӷ{gwI+}}42:: oFlRw]goy0Eat|y[۫w0ڵu<u}|\Nu1OWBH׍4ctxX7=c C:6czp'N9ʱ~]{GZ;eYˤ^\˓BH:aRGc ϓ:rcq|mxc:^ʅ\BHLu1݊Ή%n}&{/! ?;_Z_0U%gd|9r>k^Sf{d҆nxE_?'jhnj?#SݔΑd:h63h l6MK ^G#;+?gHIƫ1[켤N_[^׹Ѽ^>y4Ïx$# c)okj?.mkα/#鹹'u׵w}ַ|G IѱBיRP^91#̉Wf~a:_}u^mUFcȎA>yܥct:Gzl9N2Α0G\AɌ7[@`[y6$y۾3^Q0iI]%IM:I[˼Kh cMI xݔD s1:R$uL_aBh)y472G)u|''-uJxӹgگx~4++L:Gtnu@GtZF[et}4 C`{& گ:9M]úD.^1^I.+y] c wK!oyԑ@םOT/[!ufׁ'y݊蜸Vt:'3tNf,utn\5^9Qu} _^ԻӹoW awrF92:Αd(ft}ʌ6;t7"Iu-v|&^~zËxНڹx݊ѱa^'OZ|X:/ћJiE]f˱ږѯR0:< .Gpʫet,u.FG;R72:וwy^91xŎtNf쥟3b:lnED3\#r<9_]сt4 3 [9H_~ۯb3:$F[f#_lRwUlh29yIvԎ"u&c'3F~!Iʥ4:~xH݁Ngt<{J$RLDxHh IZ32:ȵˣ%u~ׁ':љ\u*ځ?S $ﵓ#;bt׈#d'}F_1w_p_YS_yy7~ä9r9Bڃ):G3 Hѫ=\;AUl6&u-lokooxxL6uZO\uٽn N9@#;Աڙgt<:/<;F{yԑ)څs(xPQ۫@bD"b i_~އY4tcW}JH9ƌ_/xҖѯ&ӹAoc~?Qs5t/˗}]UJo=#I ly"oH{t Ca<.w]H;ׁu:]:*a#m+^:Nt^'uFanLI؀M]˵΍c^Y?k^@#b:/ѯyj9$OF:!_,˭^#1)N)2:>#:G+Խ>ۿ~}`3:p-lF@`#x!ԎyGvxd'0;)gt<#tFǪL^2:6KXBVx.@Z'sa#I˵뮖׭x4c:G\y\;)Q #Fǧs+Ct:' Gv:\N :G?Q׊ԵE醴t>]KH`dtr0:s$OnӹF`#$ Iݓ帏-#-r)fquj7;:rdǿ"wS^et<RGt^"ulxdu )1:%u)ܧ|V ??_u #1:1]9k]uWZeo?^i)kG)tN&ٍt0F@:bur<F7D'7Ŗ<^u99h:GnЙ+n l61=qmu=#RfU_nU7‡ulYMu.hԁmVx]ԑŽׅIٍ.Lh uSRױ *>:&ː:Oyr !uܪvٙtNl(9C\5~d9V%u:'Oy0:/c+FGMIxew4jt|%:soYG}j7'MꞜX=x)Z^G<#y5qHM:#m+^#u3CҎu.X?|濾bqm[FGGHbNj׹VyNI]v93@"`ٙtU.CRLN^rSdFG#ݔut$2:s0:y&ʦsxJoɍFF`;1Xiw7yg?r:Gujzd'wNx]N4xѱ) :GvyRZj'.O┌HIzyrL'Ly '_ww͓G}|&{ Mu1:'6.Ѝ즷ϭu]?bjǯ:#ԵnEhTtdˎ;˧ؒc&u}+ԏ)y:=;IS@.FhƑ%I#Oʲ c^g:廼>˞8_ 9;2:u.RGG^PN䛐:zg9%{ [񺒣^^sb~)fɤZtR?k)\M$},NhD79zSZ:Gt~& mFG`>v@/x>Ŏx@vr5f{)x^GxҲ)#ѽs23:rЙɶ6'MЧgjz}oᗽSU~ej'nJHxّ@wj:y7EF aui׾3_џl0f_&sSRG21^0:uRG[^7:Pnd$u$uSWdOǯ+::[3u:6ݍs#k[g7reDv%IKk9ȌnEHx@G1#:G?%|с6'MꞐ@׻)Gv-yM,Gv3Q;בrdG]?~.-Ż}oOwi_&ky]:䤮cqL/CM܏"_x3Iy]ѱZFʍO:LDvh:'b_=,ecHH`EZRs1)kGgtH+W}[&9Y?M琔2&uOZ+mymO伎ޗ#;/nu^G"GvHxvc^gt;tul&uS4˔ڵjy7S1k֛GvhNZSGdڕHnԣāV:!u4o׵^AHׅJ;3;0зb#Lb>09$CI'M @_q"D?d&ε,!u(K(M[/kvndtSDp|Q8saEH]GftL/telX6{bC_xH5ڭȮבfv՘#"by.Βu-c^btu1?:/kb/v&#a)yIGv_ q5bwI^7:tRG ^%uu9 =3:5@FSx˵)}c:7=3ݔε.ٍt[-F`#@ؤAi P;NhV9Sc@jg:R:Cx[;yO= #ZR:Nѯ$uy@R1:~^: ;hnjx)fu MLJ?/xW}%O>3_?r3R<-gj'/ʕ Bگ.l'jF:GBhftHoF`# INȑ]HԎ^5a^Gj:6 dw8c[Rdt7ku'eGXud3:V>:urG-OüN>E+|_ɣPkGnjFwΡs2/:YrЍWZN/[k9W2oF`#B`g#S;nkI;]w)&~^ Mj:Vh^~ݍ.95:V8XϔGyԑ׭HG:WGUۺU2ehJ!)k5n$u:đ%._M?r@|5*u:GS@LFG7MXut>o,o wn9W2oF`##Iΐ+ u}U\'GSj'OO:RH>78uxy 7GvbҔՒ:yHV-rUC)E1^1:2:o /זuJ뺉8gѿx?יtMvs]op#O]Ny(Xܻ|7^}2?]BԎ5nq9k ε~݌.F`#0E`WC=쐿M#RLlb4]ױ]ױݵ.sLGi*?[DCxx"ׅI)g^Ǥ?1TH W)\#εuEέRwڵ闃 p@Ή:Y?SF';:Q;atS:Gۯ'[*ss9o*nF`#@`h{HݥLղѧ#;]v^G1üԮvG??[gt ag\Zo˵a&26yBhReiT|UU%y97n<[?s)ԮetRnFbtŖzVGvS:9:5+tfjG !ƿ:|=nɍF`#B`:\[#;Fݔבxdu}EҊex8^[vr "F'.pׁ=ZkG~k|?}G~k|hRG^xx4{ՑNZKbԮε0S^|/\Ѝj;^98nCN./\>}=~n 3M9l6#ؤT1judAG¼TM]1Le^oɯ_tv $o #9ƒu.RG֎gtkaRGxxL'3%\i) ՘tn7FD_9q~@'\?ݗ3:ftݫ%x#I>^p2F &uzԎO\v;z'hHx);C_0h߈M@?¼et"^Gb\K]ܪ4LIڶsX\_{wg=?M )kAt\z|>[~#&uGu 0}6&ױ)OOkSk] {~ԫۯ#@oI}uSRG\NAHTx]1&y;2aeHŗbv/ףP+sudW{4&WctNannwεqjҹ X9yxPwp>k_蔋-e\9;F't,_z[7 =d#dؤ.{81#^GoGvԝջE$.!LO]׵DX܈&:z3F:RGznNdtsyRbK;%BH!r)fԵ\__?[\Nn"PGs|Řt^εcuj\gCKẏՑ< ez(תeS:'\l:a#T!I][Q輎f"ulkG:FyL qj#\GE.hH ?8t:s1} X7Dh"uKDZi!>G|QgEԝ1)Gv|RZSVR?cj7:8#pd7%u<rd0:֦c-3I]Gh8Ubtu1Lb:rBD0:y;C) #C^K _.bsUBsSG#;ftK9[9b/ˑXG:.zN[n:K=j#&ug"Q;R\9n$ulN:L^Wud:u8#m+^&uyL 7bے:4@F'SGv:SI}T{dBq#uԮ;SIҹ ^:r<>7r$е\>[lA`sp޳T"0R;޲; szdu$LZFWh..;ѱ s1:6 c|]N՘ԑ׍nzʝ)#;uR;hN ōW/|n\#z9ϻ{Orc:>xHm/Ϻ3ssP(lF`#p&ԝ uO~x|oݳ'dvGv cԎH;~,/KjxBȶ ::/R8Req]MIˀGvF׵9׉NH>ydsζ՘ќR;tj'(;7UQstndq2QGZusќ9z}o-7+̶F`#P&uE@n5@vdԎ^͘NHj'p<.;ԑIGHi^0yof=LRGj^&uok_r1hم\ks.v4`5Rќ)t&jMDBDFL:GC9sEsm9snF`#p lRw Q61ۃ;(gwvv #HԎ^tpyx#;Ա1]ѱ*u+Fӧ_Nާ_Ngt\3::r;NhrdW庐ԮhN'xL9%IeHDȮsca&u,bŲbl6&umI c-/_ꞁvSR'ǃnj/ǛyI 'uHV;RGb^:6`ud"u-Oc9@?4Kڹ9=;uS]ǁ0{\;vLйȑ7wNNz쎯p9y!2FfؤfC S;(E餎UN9^wd u4c{)Nؼw}G׍gt]3:V>:M]H(Gv'pn7Gs.F2z_{>}A&aj\^(MWft dq2Ds-{zO}N\7;~s+3:ҒΙFؤM"0>vgϤvccpj'+=IkE0c1PcԑRӉ\ QG#ב]DLn"iLZ.׍ PZ3IqRsYt:Gm?}wq4MےF6ؤ6㲭*@@vDmotP;v& #T{}t~ћ cGv:c& 0:AZF )1Gȑ] {O /?c~eW_yQڭk^:VbفMŒu2cRױLԱHGvCUq)ӹV񕯉{__R;Jˑwx#:'vWԎOڗɛB=筟ҟ{e7|s9lF`#pؤ>Fm\L>nݮ|ϯ|yGuc7jD{k2MF#;ԑ؞H\,щr5f,|dG'I]Q\P;st悂ۣ9e8S;z؝LN-s-#΍D3q9yӹ@!F#I}඿yB_~ۯ]zK៺dݵN[zw VGvc%##U&[}Q{C'1^:RkyI8ǯoByL*|e[q=ќxpҹnpz4׊ˑLKkӹskl67&u7my_~@쎩]:qgw '^gt9p\;~ :՘1Rʙu/ŵCFbt%r2~v1.\9!mԊu%)NT!\#r/͟{xsbGm6M˰nhbvT"ŔS݊,|5JN9S7̣-Qԑ#.F::%rDrJnu(7W!u, ˍF͍2漝d ڍSXsske}%^F`#x\ؤq(;ul7{ǻˮIJ_|WS[udtudnjrjԱS;Ա)x"qw?Mv8c/:jגN9s%N4.8δV\کdS8^͵Dt=_7scKn6'Mo joogɣ2/1ԠO#щ 8hpQ;ԱBڹ]"u ׅv^F'1^ ,WI3:QV~9o gwgx[3?ˑuq\Ai*6&uOjEHdBN7jJg>@E~-W^:v&@b4ˑ0: NMGXv1R1:{^Kׂ`^z`cdFyM/\vG+RםˍqU+@5FIC`'-ChMОղ;uoΗɥmzFANTK0$#$^ 1:|9ow?|Etȷ^%r;Cddwr47No++.:eN'rar͟P:6Ce6&uOxlX}L3xٝ:Qn&+zsc6H3IǍ)3 u3q:餮%rڗ0:ySh;ѵr-,ajG*nS['tT^rR'Gs"|EAG6Aޟn6&u8V[r#" Ԏmcۯ=q<ݍ;~ҋᄐtzBxd3:zv1R ы`#"^\$ulHFF7=fZ1֦U8r9u{QG3Gs_/#ɑћ}.7ͫF`#lR3d#p $ GI2fEkXlDI"dtB O}O\Nlj'N?[:vSF72ۿȹ>dl6&u;6"s6%xkP>3y˯ܽ]T̿{x9r7k[d\:{$6]: P;qup3]~~xѵ0 ;5\ )䍵;<~t"GbwϿtw:o4}B7n7c&uY@;courS;~ԎIOv<h݊э4}nԭ\ k|{އ[6۔&.NrY9!r#Vș6F &u1@% YGѩ]DRщ;L.g}eԉyp%uY#!L襰;u,\Ŀv[iҊdp9hV\cqӻ63~ l6lRw([Fxx3\4o:K NN`BLFgR~xg[at݊с,njS;/-t ܊э\ndq^Z9އrZF`#P&u`nU2Zv:yw^ȓZ:7N)owv3-#1 O&Sgt2NpR k2F|Wu{(tqR$uv-˰y.R+$M_'ow2ѡ;kjZ.bqӲr4J9ۇrfF`#&uG#oj̤2Ѡճ̾|TxE=SHٺy ]Naql۾=r?o>qċj@>,^"8Fo{yɯ+Ol.Wֳl6y6c5lNE`#x[Vw_e~[ ts?,I?N݁'Ӄ6 >jg" -ɟC؝Nto 8ah_7C/u7K&l6!IAnп}{3W~?E 7 dqD7W݁U{w+V..vR8=:&\ɑȱI֦%r͟~BF:\`!F`#plRwm խ9;&u Nډ<cFZ" ޔԉ.ߺQfu,nz H-C"7)t?{㸎mhl6F`{mF`NhFR#x- 8]9HtR7;~{tQrZeΑ~NIzawG/ il\U8T蜋*N#lF`7G$x=x)ٝIZ3R'u ,$f2V-IIۺG "Ǚ$Q\oG Dl6G`6o#P'渵OK`A.t1)7(cH:7 ]JsB$`WOm:buZsꎃg򮑘MYW]_]24F`#S6foj L֝~s96Q˫㵬];:dǡ$?D.Y\p툜_-6 ul6{&u4pQ(4ﯼSi; :~Jdo䑓S;Q֯0͟MiICJ ~[g.V_91>nytuȅ- l6=q^lE{GcF7r<"uhMYSӉ13,FΗŴP[l&oYF`#xLؤ1 vk#p0Z4Ҽ)F %`w4LZax_8GN\o1-ְl6O=9ޞnGdzD:bpe{D`3 6j#lݿm7)~_?O{-5p,kj \]=F`#<lRĆ~;-/0(fGݯ~Z9Y-l6#IC lt]=`jDF`#W6vo6F`#l6Bm6F`#l6F"Iݶ|#l6F`#쓺F`#l6F>#OsF`#l6&uO| l6F`#l6~}޶}#l6uf_=&nl6F`##p]z{Sn2yS}c6m 7F`#fFM#k%CAdƲ 48Wm9Acu]cQłbKk՗ &͔ȵv֒:WMrrY;*"{e]2}rF\Eb Tfp8 LљDAHZ@h$T5QƵ4)SBr–ZqcҌpb EVĪO GPYjkBwyDl.{XyҺ$\z/8t#ɅQۗȦeTd:#bQm.c<($1 }+sjo1ft+N踀vD&rE7oA}IM%?#ĤQîWw Cj$3<p "\1!vg=. ח^En{o!8+sPpm]m3K8<4\ j }pB%G$Ue$y2XRyNHyXgo"s(AdAi艄lӃ_[td`r SGU#u 38\XմY8w_µzWJkwisƬЕՌ\8L[%X!;4#J /s'?TTn 譎7F<Z]#2uBD q}߬$в'u6vNNFa ׻ З6%sBfχn/II椵+3d uf:0XWIiSqivLc|ܞ{ڄ@p)0>;.in⏄฽l5"l .+^m1vV_U}DXX27\qp3 ufbzހ2OtGt!8׆K1 VmɱH1%4OD0v"Κ2.G)r XWJXXWyVK]f(Yy_¬qU&Gxٕ԰rcȵ*qO^./)Dq h_.)"pr8)\!Q^J[*tvrYt꯫*e'6hЀǙԵ!1eq|Asӷ.yp7U1=64TFl9_R$)!O@gK y3 |FS{q`f#CpSG7e NtZnyJ&I~xwior@{ܔV:݋F9_|0ҰZaqqIoVm-+ fn/>0XV ?yaf]7 7kQG5v TR+)m&Gq#wypXUh19!wnԘh{RpfW&)S )Ajxq8SL[ضÜ =ֆ x+v.|=)gp̂f4׃G4oK  x*T%lׄ1?k1} i/+!pP>s%N\xZs5d4sYjı=Bһigp>{ƑjkBu\ f*IxƸ 7^2d qpp\㖘b4o-ٮYd|ARʬx^6 -|(Hѻr8\N a|d4 -m@2rո#?/\2TfLGIwhHfDt":2`&+fS>!e*[f+c&9)j 2݆JX2Mtȷ.̈́mE|#wRdesAVeSmw_U t9Hm)>6JU

d^ZMwS#ixmGOQg^Ò`♙hx-ALkB 7_r<\Nk  n. c6#J 7-#7麟ĦAXeJD&7x ),15u 7cqLJ R#nTrjga])t]D3V%d !#DHՠ$W)kC3H1e2 % Kؙ׆5or^gף1y刌vV׊د..[A)zBo*J0M.S?"Z s0 hL3-Y,x]uN*[dp|\r@d\&[{p3PAKp1GH<Do8cOR1yAS;]F %<+Y6%1ijxR5iXȸ\(Qjld.3,wJig:h)>{]7nR}"(zKW3Jz8~B|0NvPp:kB6O۾Yn_A  dK`OrY\ k9O.@X9 9l8ˆdW @] 8GKQB9𴵪B?W s @i @XK/ZV!#f1/bR"22Ϊ {[Xpm. .k]4P]Rv%#6՝aۉ z(:Ibe٫{` vHZ 2x]ťSdt+>BU쎈/ⲹ pW̨6}WV vU<=.)YewqNU7qQSgpRvTdLuAwY2`h\@-mgƘIn-85ԆNՒԵKG4 0B| l{m 2 8),z:Ot!+pXYJdЇ+gZ %F&eXGeяDF@%0tѹ"fmUVt"2L4͛(p ZE>ޥ$Jna@Ixb˶ޮk|SC8S._uIdڏ+eVn <sVtifqu x6>l #9~*H1lXuDZDʒ [%3y"N#cf) zG3m^Y٭{y2 4F5QX N!3 dT4A m7V>SyMde"]!Ra.\ScL/L>S7錿f}d|X߽6ՕW.*߽M?J XoNCk@ԬpSBsj2+X7`_٬ǝl6SQ)k|\ά8S%(Ʊ^mx'r!n7wY^b:Q5w2kʡ G38;ͩAVPqnM0fCQljf:b"봽9$Pbb>FUᳪtf#f*3vv(o7@` aRgMZ{>W6{wm ytb֗"n*p}Q3G 2L~Ӷ@]o ! y+"=v7x:MX[pRFK< xrx v/, Œ >M]YwǕtx$ CGwV fq+>y7<:N3VO%!.^2LIݨ9ShaF1o^ YCxI+Oy)_hd<1 O6X`iNފW iUJ%.bސy͞@V>*2\I?]T_#N9" Дo:!F*>W2 \`:YL:nX ԍ+aE̸JӢ{iGe崘2 51aMM4Zn@ci7۸L"hH7Qjh.jɌf0`#%(9.:( LC-8Yĩ@5>ЀMwYt;'? =]L?BbymMwTdOĕz18ӕw@٘ʬ8l nJ023Ϊs%lP>b UK.<7gf uE f2 \G8Ϣ!69U:ֶ|w;::}IL>^,AX\2fUU{4{H0A2bN>w4׎ Ďc8|2Mp_̠HָC}O(YHXlҼ,\ U-XO *m%UaW cy8 1Ll\56kT:hyGR 8GWV3+)jeVq".twanm|$le5O8>J]Ȱ|r pS~*`>5 NTE | ]p*4E o {<0p"lX H΂ Gd*¡ -,RHkqm>UᾯRPɿ.{n!mbXJ4(F9Z5޹@L1"Z2+o⠛._;Sd`x}mSpRx{MJsmfp-tx{b,p7ڥg" RsMWe;su^c KxܴZG뷎[-Juͪ*ٻ1J7P!͙d' DKg@ᴜfWb0(S٥^ ccPڔ`d \Sq(Zțyvj` rȤ`ptQNcZNG"(S5Rզ,}ȵ\?λVh4Sb@Ҥ[, xRpb魭)Gq|q<ͪK(B*PR5gb QݿmCYgqE%sde"*왉M9lu\|ۓ.%;Kr2BK ]eつΛmn$Uq)uMy_)se0qe; ˜҄.)O-%(g-Lp6eHmG %OkMuX04*q*qzMw]'pʙG.E5޻11Tcb+3]1gSm˂\vC3i⠠vbJБ >hTC,ct33qE A c$s2JMHv4j(Ľ%q=%)S%U!>ӫ%FZ@8QfVX]_T鶏~%@\ns6@XkWd%YScNP&6*`u%]f 8\o1wQζsN@: a:f܈aX3uJ^L<휪M*ީuynh\%4+j&.=Ц*3Cb%:Pc{q#}_*҆ݭ5C,^dF]f )OZ5.%Q2d9ynm.+M֜"#;5q*6|?b@İGңU-"^Եd#cv`|t;T9{(e*<]93`|xmvs5t.Ua|8 y9$*Eb2]\r©®# T],Ryv%ҸMr/GrlM.6`\N)p9`f촑7bmYZn3bIr"/Цn^۵4\ԵŨ0J{ K,k g\cMF<ަ!i ͢U PЮ!<p #KL+`v1 &oǻTMH9`dfAJB Y~GJ.+xEZS{2zG׭G5a+dYsATuo1/඾5 UXf,@Uzg @GtN[Mm.q۩v^L(^gǂbIY@|?4d\-/<90Y֯jvӪZBy0}]unn\bz`UͅEQK+5<{N%uJ_鄺-dmH8;)ScJ, (kb}אdK-ȖNr\" +_ i{v /M* $ @@] @0*GM ~sy;DIfY(Y;]2o'E cFڋdx:DGs4 ʼHUR|k|dU%5lJtSumS#Eyqn0# @M sW@qp"+8IPp䕾৘2<"TYqnR^2kQG?vpD17 Pү3;hd m9 p41ṳkg:W^˒)xwC+fW*kCw9rtNw_FC!p3M刌9QX\0nMAFpF+b CLC25;>v{Mol'l9.'xd3ӝ4ldt]>_rU.ܦ*$JP]p, G1<Um6 (즀lrJ9qERαj ՞t}i-Q,a̲SIvqip  k DpFvh=D[U9za.aа)@ nm>Me9qz 0Vɴmߘ^ƦYNx1ERiVqkLzs%^ !2HǴ<}$v`:[Xb۶" wi-b#%98(.G޶6%N2Iᦼw% ڮ'W!3.vYU<[= HD*Θڅ=0yd L͹a2N[*w#=೟ 9.4sҪI@6ܵ ;lamr~353 ~~įLތ.~.smہ;- s A,ī+o:Wՠˏ֚&UmI66ʵj|4Pu,Y'F1C/A.=^L0W^H?W~v =u&ʮݮ_Ӷn2Ȱ5+0*06%X[fgOzg&bRr82ܛ0<"SQ% qaJItC\ю(6oPՕnMYGEb s@ Xy%漲Eb}[Ig^r) 2i𦶵sMw.搘Q<֣花싵66ܲ[Q;Mm&@ܦKJwykK Ka`9jyX$yn( `e%pG̬93%\.#ˆi()&Q6+iX@B'`ʃF LSR&XTc^wԵpyK\7+VU k H ή_D1/:T58Sr Nb(΃ I&xyE;Mfp$'% x5*ISc%AzY(׹RiܪRQa TbD%(Cq+WxtYr'&7?f>X+V8HQ0EHpmj/": ,۹o)e!W2G"z¸?-Z!wyJS8dBJ"|kWD)Ѕ+qW&ɨe (bJbrX PB\Gx+!G q`f cfR fE+6dR(;89P]\ULiQm&K&Gb3 N: yؘmMG9xb,k׵.\^K-)ixurqa"H4hBS%.(ѬA@J (n=!W X<_}HyQQ[QK4*ήF-nvRMZIrNnz5+.0~j*T,((JųP$7}!#@Lʬfi=yaS dYP' zӱS7gwޕ+ ]wM)NBeݰnx3Q->q͟,f-@O׋"_ۤ.ckt?ljv .p:s)JbGH@|ɤwV*TG&`b+˽yq w*PUpr @\n 87nj| fthZTNf4$fwE6 'T lC;~{D \.Lk[J#)qTȬr|n)&cw.`)(6:(PC߀@{esBzA M~1-1{t  ^wO\UZ!- eո"#tu7GE? JMNp.21;B0 _)]爴5ZU<]h%KLތ:@b*jIJUm4n aU^(4xs* h5ԅZ P-(F! >xY2X:SK\8'91G#PPdEڵޠQ^+"% %g@`l۱(wr=1Jn[?#3y<#'|zU XcG (q Թˢ zodZp_bCzI~96}}d̶Ţp^W 6#rhp!0tyLW82p̩yF0޵}^v81h-2[4J<Ij][y*uctdU4c5V S8w|b4 UΚOTaokq*yyΣ*^wL#73ӴjUaג|׿V}^% :4wV*r/;pm>@BAc Bbn+q8z!-`΀׀my S6Zj2+1"ެ>Z-YRc$rGcK -Q2Zrs-@|!U.'wܵU"e4챱л Pܬ*bGdtE@V3=u> P>6lBUz1lOs5 e^^ŒՌ,Sc!b,'g̢kXض}t;52'caXa\ZN[- $c?D:X_~Jg\8Ir|A4Hր|6A:ؚqق;0+$IJ>{i1qoO]g,HY%Aڒw]BUqIn w':![Jb;Bٜ_^U Wyq*YJ]zɪsݙzkj"7i` gW,ajL (J&u)Y'GH )7vTsT13ayWm2jy]tveidk WcN$2^T^jP2-42ŕ9 B <]çFtv A ;H&f-طzg ~m]6=ZM)~}.vzDzKLƵ=:۹Ns\&8)` \ԅ}VQ[X?y5,50jȻfJi(tq3 7h|ajs2zNq6Ŧ i#kή{}pd So8[;j^\|xRWPYӝTeO)y:qn)D̅P Iۍˣ<1(VFLi߹m<lSL;07/\Z1/O3~@(l,~mP]T0!l9h.soK;O*d(ɠ XhnicP:pe,\/*DƜȅm&X1K3Ch#gpvA b+S2Z*'da/4EXZD㼮նNRBTEJnV|^uA]Z\S⏙5{Z6Wͬ^{p<;MrC+O)%7U3q u t L6[fR0uɕ ^4Ħvܛ?Sy abų^aZc/7SwC58oc9ms!(x((AvS>NZ$/\1N!RJLL\l3fLC؆Jyx,HqP!(CQUR1rQW9+/^fDmC9:.CS,9N@ aWlU\+lg /.߫0Ph˩" g|"2^GN}ol1FB$o3DV"L3cّQcN`Jp N\ ;džm80*i۪Ԇ ^Pa,R\SǐW|*۽\_1OHҒ4dq{$n7^^nw+:t<WgDҩpW,dHf m1b9v$i V6LZSMKVC-%*0ےB ( s$.HtX?bE)#`dr#\ŔIc:{*e`X̆}Y_]3?9C0C V#W6mI3(oOZGdԒyyFЉ8KdA_c 4ih_\\,Wn\E܀LjqƵc[(U!`B LgnEf@o3̙Q]Op>^̀X7Lb}9=NmY'0[G ˃qܓګK. jnIϛ˜vY"Np "NZ(1kjӯ"H]AQSRVԕn*jTW\Q3iGvNg1RvCI{%jV'%e:oV{c·J K tWXg1L|f5>z7Yykl  D9VʽɁU:5E&%YK-8m6*[ԝ9{|!UTN󴳟=mv= \Uzs^'4m.\,[C%S/:M(Lcrh>vO b$t;0pwS1 y&NR)o Nj+G.aDƅRRxQhwB]@ᒄY0ИN!"Vŷ]2+/BA,?{yÝSm=RBލ[: #aUvE=j3ӻf +r@&gEs:O_wF~)my:%h%JdfwFd@/22cO-dRDħVLTwTD b-.K:H?)`ɤ.æ3Nw6(6]V^ jR}+/8<kOmbXycɠSWr],&CZ>]RsA3P*uC~sHdzdF]H\QmN(׌p(@IPt9v6$@>hamD\KJidj[NJz5ļ7g4LV7\\S{2h:nRE9u=vtg o~y'&ibTs1%8PU )t0uN]e쎅Hz+h۝rWđr^Lʭb5pq;p(Qخ+b4 yǕ]a;ԑC EҦĒ0mʘ`H&8t,")ӽ8H]WDرozX0@ 1}_ĝ(ŷ 3duV;Wx.S. j)3OXwDGPi)qj}pWfn:W@M;?q%x %-X^ JZmf`ص@iUTM9DW媽+W^hU ft!R7E A0ɳRpg86ԉH.&SA1ݴMv 3#Zk'# B4AY *ǭ-oq~I{=B!r{85av㼠%Ukko3 N&Yz^K*X}ߴ6-W^{MO3p]-\ȼ;bF0F7rir@kS4$G\x#ڴ2ofͷrDe (q@L+=Y7x  IF="㸻PL'u-&`2c{|jUP< yb^pݶrp [ ̺#Cpe| #S :sVW AԹAW{^;|kAO]`Į[lDdb㻗 oj?0K(/^X p\S#P#KZ ޾T9>c@Oˀ!H-\Uf"{tu>xýI̓){A99߼v`]cN  5 (Htza>$u`@A0VD䀮\iz%QoƵM4sX]2 7{yʐ$VY2.-wKIO oW&fhG65 RfϜr~Ao=1q%ms-Lշ0W `ݶU IDG4v6[Qm3먂4{Tx%," =1cWR26x0c;-Ա,9-.c\xf=w`ڞi]rXb!X`芬& 7OA]Rku!OaM%j20iiI^e^tǛk9"{J̒X͵ %2ЃR>&|5\ƽm]*9+,kC׃fa- YR:䆄)2X <,DyLgdE \UMi_97Vd/D7UY SgaAzlFAwufc<$ #0#Ow]I' }r Odbm':U~Eo:%uy! /鸙ާk&g c]iiyF)ƗQk6Kic 4/WUY85u88PGyܘQrTFcEuUWO0l17(%  D aǧr/ĎmAآdb FPV"RCJ*S^mJ::=~խED PM $+ 0)f9XX#~_bLC lb t]RFGGsP` 2"ˈRLϵFuj󊹣WuyhK|"4p@IP L}Gb f CCQ*i57д0dPR*}6v(Iٱ%u+ʫ +-\H! ;iԺ7X^"r?JL"%f8?vj/8m/[\ȮLk B/^ipg;jYy17ВKl*s.DXymD:UCo\:zN17.FL0W-o0PҌ>:M}>?ŌKż|%x~Ť7/>C0i=F!rhAL֋&5j*q\ Ϥp%u.\!qzP!h^HA%EF M[d+!6eL$@mtrw>sL>4~ԮTTZ)ɥLBN7}tOӱbp9t+ D:mcp!ѫl< Nx2 Y>8/A2 dVV~-6ۯtd|1UZCbLOVS5%CS)`XNr)nߕU?յ:UQ;*%QlRgڭq8!Ju^^<w|!tnVű|1!VFITafpu2\+yp. KHvc%֚+(.ᕑc =? `U5xM+F2uF;*sr)I}b [[[ Ղb]$JR'ޖ$DnN'u˟Xh^, bYmZ!;r/Q6Zd[O4-@#ˆ̸̪\8aonÔim.2gkz<ƞ~zk/ĭ$߼Jڒ:0>D4zŕ• a 3QU-|.SKai_@<$"hAꐅ'LiZ(p)} ZyS ai :t @@\. qyi6F{VsA׼0"v`uQ,\u1.x幕 DZgZUSGnԿʮC-q)_ $I\Nn e'n;k0*\ckcFN#`*\i5#2zgn>L|:z@9bYy1ڔ7:1`QӶd뚽xd%6%a G? ~.(s);m4M%z!0e 8 pP¼*¹ B3:^-0.,;n_L\\\|x~"\C2q; %($gtHIOĕ<Ԉ_GW6<”>E1L1AjX lEڑ?ML#)7'uf)x vmt=62^4Ix}ht[ ZpE\ ?WXnjTfٮ%OSa H4u3cz&+HC0W9,u  s!MA:p$3Ck4OhvԵڼ ]-]W& 336^Lq(D`2xC`O,ǭAD&ki!]ϝ/_3eUyzbZ796֐$59H@Ǫt%Ad4 /IA G_^#=BOLdZe 6tbR!/l4O-T`ݨ_X ]V.:7༱ae 㖛J /]ZXp XA$W6gƂ49Ť}xfu*S?NISå!AZڭDd!^ wH|~MHt:z uW1IRۅ,`fl^(Rl-&A}8F?aǻ70Db caͣ4Tv`;BpN\$fC㢧JayUҶQnorճPU۝wmeFa!\1 Q1u{͵[phDd bɝ+09;EiNHiC),Ng9zhLǶ4^޻\=-WlMȕ tfV[sGƨj/͵Ы5@⒫ӛz:/Acu@oz(r瀅J@wnXA-lv U"kd 0p@ͅz:H@446Cj#RuUM#K;^\6Sh,4>M%[۪IbxUZӴ ja';+}''XIN6u * 8g?twW5VZ k q):$' +Ū嫏OAЕI~Y #6+KpY>McbG@zΘwfTNmA"s(b!"&I@U;$0|4P%Z.6v Z tlmJ5 njC$pbҽhҰipCdL  +b~@Ɓ•SmOj-uufw䐺nCJG7p7 ((s( <R5a[+ӭ܎VVV1BF9 &A1᫵ԭ,xU4o3)M:o>TIBpL@Ie^BIr['xu&-,JK*6ՙl+:,A}# ,켓AqM$X( V*j;x b4u)NIl+A 0'2Aֺpf")#xצi$GD&`Olڤ\٫hN*PQ8+cfR \DXU劌R %M gc)'&gKҶL*+e&Mֽ$\` pF/,JbF%3&Z;U9d$0n͵CmK}l1 \q-CGJ ,^vSGw 1R8Թl2 輻8na>\EK5i i3KiLF@v޲d $ 6lj 5uYb0! DV0liK.R맓W8yLcES2js%гeO*36î,2n=[ 3Xno 0p ԁZT,-@RD#,s ֚rc 6pDfْ4ⅷ]s5[SdGqL,p˽rapeߔ"f*dxpA[).:+ЊBHnIaX̝ 6Jb0tl:%|Ш)Idr}i ^QH#u|W!p@RA bZ!a׵M~AKSn@TkvaR],j%sw0 rkK-MtMe,@$Սh.ěJ?sG@ z5uW(MpnsLr0!>k*D ;4g2N+ >ZGsD@YV@Eo%$̸NDXE R0P7vV `띨ːи̐\EMqd ,C81p\5*fL,.{%,.I  !މjٕMԝM}L:$pzf^,cXp"| ;Dֻpk%GkKhL L+̒Dp]'u4e-߃wA,z[]6C1$[p Gg+_$̬)Ѕao2^c(lnEL䍔)oGר"K:iF2DxoSia^ R]Ar /.[.3ExlnɩFIwD <]6wa~Dw/WlGju6v޸qr8bJ%V )SY3cb fdd:NqLEa`0f+0zaoKR  rs8%^̯x=mADLA0ͨqj$\6/G#-<9k]g]GJ8w_HDڤ=#L#LxJ? *SӁ^p Wi2W.E|M4^) OCGe&M}jaɢᝮ|:upZe>{g]Cн-c{&y%)>&׈PUhŻ+UY@!s_1yzG<E%޾_ჺs1g|C y.$6߅y'lQx,b,̱L80dRDf510W w:5gfK-mɣd5"YȄ q%^"SJk_1Ȫ<.:]V5A'u&Wi@9r3o)7-$ 6${U70W]e3L@P8q_eb f(0ܜ@ "c΅va)Jhb%hx.(zHfeesˮ5 ,ԦV!u+Ga#:L:uRǭOfv?ddlUC-DucԢ)g]v01]쒽1ok. Y> ؍zWA -1ӵ*+Jm Ѩ݃("`@3C0MZubwqyAFXuR %x)K0F©uݞ 5i2~@V'0. 7D#&%R1mQeڝ61{Z: W7O My( ( YV*mkqNgl:'[|I:z]O2(Gxjvp#6 ^w2djW3!`CR׵wLV;$ǀԍAZr@ODܜ\J\*YlMҍp8>ɠ)bQmk-dU?AfGTiFf7eTuuVhLB UPNWu_na{؜'0bI,.vex3+[nCM:q)^k$El,xy;XZ@Pf);f!J 25C`Nx\IMySq__)~rye¾6P ux3"91%( nYk3Fv9*7{%{ @Ӟn 7[꽈6 ΂R"0|ܮV D`#vǻdJ?k7*y["+ٶ=f(qu0Sb{UlƜ-_d40YY^:#/Fk)o xQ ` Uu|ADr\ލLOā꺾\x ,PݞS +e2yIݫ<*D4 c,&%s1q f!?dL0h#b :1"D mտ05 e4y 0ߌHw]Ə&4a$I7OA5T𧒺):yo5(:9+,+nܕ!yL =ͬmSF5MWԩpw6Jy{@sq0IMmvhӶm45Lu~3)ǑfYfqu^KUS\86'/ F=S@;+ `{y $R7]0*'S7^{ &b@`{ŸGxcYLܞe/F \8/ϕyȤb 73צ IW'rEBu'kj%.dL7sVRk oDFG`ēin^NNh\\:cz BIvdh2Y8:%*BFh%ov``@63%gIO3mu,8zg nچu~? c`9g?/e@c2fF)IRfXmcR#|t9O˗K lŽVof̒Q҅U`<ւ[pP  ׆KSߚظ ]@~ד:Y %U7S-sg> %SØٱQ=L}Kz,geC3Z,q\) WL t9F \2Al>t;w!Sܐ2MV͙ڪڪ\&_38pb:A1Է),QuՆ'FZ/:{*I ʡLY]ZHy03\ 7<0Gmcڦ{8(z7fhSAp ˭*AҠu䩽Y".[ɓ@np!hm;&.ɽS2^T{Ե/ԆXiOr>XC5` ~T@So؜Lk#\r#u=+ģ$,3NEڌ)%NS 0s#|%-9}i-ںz˸]e)mKMY8SNЕ.\f|#zgAnM W'KrxZƌuyɥ'v5$=bceAݤU8B xpaf)F& ;v36/ŽMY\ŝ+IvfV;9L.eژ¾(V%{b Nô16eMsS~=v/DdB[XEMn'mVoHˣHPQT&:͆٫!]&yoJ$AuNhw8;HT}dVd̴Q׃cp _%!א5PkM|pIRSk۩OH<Ќދ;jjX~8͔G+/b` &Qik@c4RlT)͈ٯ%G,#gTGԙxKdb)DMR k׮R֚[HۄUC5VikUR~uCڋvW;fqISqg?Ξ#4w5-9)G>AiXM :ox2K &^t)s\Je%M`v4/R]ئ,UϜJӝ\Smj_;Zӗm@IA>{L˧x")1XR~^ 017cz+pّ q(LќJ^ّτ3s5g ama${9ʀQqJAMӆ4|1սdLz:&_&E4*hk t-]Gs *̛WhvY.\ ;U`Xr~Vdŕb1Tc.R.![b/UO⸀I &k͌oIpdSVxC륳Ag ຆtEqtRpc2fv X+Ї( z\KuxKU/{ot3g2J(⨲5oyU# u*N]Pw 6.lW@წ,R)ѿcq1aϘ"lXeQgRvp&Kl84F&vL9. W* Ԫ[L;*So'a+,5al`;qA0ց!\EeM0gx9k/ +tLǡq&d@*\4Qi=c75iG!HbA6R/Y%~(bVF~ΣV8]X0;au*7u9]Kv #If(ցliRJ&~v QR1`G2^co&{ab4ЯܛJ2D9BFdL, q$@DX&3ҵ\wUiڲd (BդҐ8>%RU! KU\@c==0v"KRF]q`Hh hst\&cP'o1NoV]I.]; X!#2J\" uI*Uzo{ * 2+{k&/Y% a=HwM٩KBِtv"CLt`gguu0|"%Í6)Te W䌣rRqk FZi=J̙Ih9]\5%q ۜ)ǥ Cߔ{zE7Wc59B읚`;&Cțخf:4Gf^%#UE?'Kϭu` |ˬL(4mk#$vIENDB`PK !Mj%%ppt/media/image1.pngPNG  IHDR!J5*sRGB pHYs  ~IDATx^ }VׇUz>E؁ͫ[l@PD@1vQK" QDӼWORj^F\T7=1c9=kfZ{s7w|H) R@ H) T{>ϰR@ H) R@ 9R@ H) R@ DX?9FX>sxG잆'rAO쓸ISZ|U|e=iOs>=!,j~R@ 9c9\eBf?|]TӿF,j^..5zg/tb[V*o;n%0(ڎ i~ ąo)~Fei~- xJS|:_n;syV;|g_ׅ9u;R@ H=Jo#3??F;:ꇴ,YYѲX'?^*ְ>Ug}gS }Ni |pYeӳ=p|3tnčDy87} :OH) -*ZHS;z3~j_ҸovUwi"bo~7 m_wayɾHZ8SFeAfCW<>=%RnUR@ <9C,W,OC%ԉjU7ꢅ?%W쾎r+|զ07i8 =9svSqV=M9j{%c5﬊I) v*pwt??wa^'rk=XV§B;ouk7UqSГw7t',#{BյRj _3n-T@\fo˦v>WG[y^}hجnR@ H .9%ҍվ ՚EտwE~Bq]E{Nq+C/GhG{S ?h}3OM.y)iE˛(W4zIwMFMwa)ˑ ꚂR7~v޺nE ˿<|-FZk|I \_lqS cI+ߝ)Z>[ڪOG(<,: s~ÿݪR !LuCU:,RCHžI/Voa'iG#R@ H)|xPʢȥR) 2,R@ H) S@N) xu 19OCTmJ) #( {ՅR@ H) RPC)vR@ H) DVR@ H) R@ HR@PPʪ]) R@ H) #( {ՅR@ H) R=jW H) R@ H) Adu!R@ H) x(uڕR@ <YyYVlVB6Ǿ~uTgq]髴mmvC*#-l7I`Q5b?6߳_SO ` C?BS/*]V?Ho՛_n;zT%|~~]KoJJ) nW7o @7*6«D@*k/. s ^.C)OGuBWlZŻ?6~`5+߽avwglzC  bUVO٨Cs^sxGf1wT/ _eG~rNgMx_p/$TR H) A~2snWIg}kV/zE%Ut=|k[vG߯սaOZqkG%<(fl o6X~t;nq}ÿnPxG,l_[+*_ngxHuvBTͰo}c_;#uZc[[Vl m|3x|7տIg:όH) nKI/hTɸ1߂CF;5CVߝM?uH'fT,Ǒ(sORV*為Mj_ H)p}_'zK+<]<]2>aݡ&\a!eW-Q11C?xM6}'4~bpɐ}Dau;.\.Szad}RVPߺA+d%-, j߽K}Z}w.hay+y0/T1ٸ>u'>]- PWlVP Ty) )P'm,V 1K7?WkGogˏ"P? _D#~;4RK*0.Ws#ݬKG{Ek~/IcWt=In'y}#5W1) M+0;~O=-Pٽ 248r' oy*|5$q`~`uw-Pz/a[`h-A)V.YUeTKMaS/ꢋKD^|qg{c#\e_HF$|S =I"R@ H)p \nv L H) ^r^ɍ0R@ H) x ȩ{UR@ HB$\+jEfFc "bR@ H) R@ T@;S5) R@ H) -( R@ H) ;NդR@ H) bR@ H) R@ T@PS8UR@ H) R@ ܂[ A H) R@ H)SmiS8UR@ H) Oozi\%-uܩ?^N(9utS-) R@ H) 0B /! gX[-D"{۪ƥR@ H) x 8G:Aݫx4H) R@ HWp~ J )xas_hsز>;5 iW龷`;-4 vف H) R@ H) H I$_:s*9cJM*<|=SJM>,RRgAGԕSQa) R@ H).9GnX,GК-P6tT_ÙʼnK_en H) R@ H@0:BFX*)}<4A) R@ H)l l?:q{@ܴ:]o*LSښѦVݬm;*/R@ H) >qe}]ta{nb6Aݾ[ZR@ H) R@ T 쀜[ A-?9M H) R@ <M[.X8/2&-oygҏ94 bzזtq) R@ H) av1 X(W K H) IA )R; -1q45<:5BáBR@ H) xRʯΟaǂN(c-{k8n]933a,qƟr9_F[ tnH) RiSV` J-gȗXzȷyV8g\~[[wߑewOR@ H) ^}5>җ.,O -MA7O}Eb-vDG>1R@ H) A~y7E!=rWL:[c<.vhLq0:\ :ޏ+>TR@ H) x ȩ{7YCܢrAS4|C6B&)e0,z>5Qri9~G?myTV H) %VE >yN箼G5۰Ӏv[[r9^E9É<ՊS H) x ^] z=Wۊs.ayoblU.2gDG P''J H) 7nyred9JX#ܬY6"r#A ]KriG :K^_ϫR@ H) @R@P*-nRS<_Q֡ݹˆ[X / x|ׯuݹ@tPg hNuK) 7fnyD|bZ^؜MfgSfٞZ(Ct֑ytUWR@ H) + ӓ`eX҄XNoՙMǺÕQP7?UP/$tdщ^݇GR@ H) nRSwEA=؀.73".+/wD]̋TXt~Ots~Kc9;Ѡ.h|KgC_?CaI) /QAKuLĹڒ3aJn+ 6,AQ"(a8~KqeDnFtarsh^zԽ-R@ <T_}_K JgZɸRKZ W{CAG ;6A.t6 %0D;0g3Rfݵo#R@ HZAg@Xⲿ.l5ҸJNIY, /}yhgU u>ǖDcj%/$[N\?WR@ H)|={H>ҳ/$~eXْV;{0Ì;Ĝa]tٗŰ 'lYk!Noʁ߂Mb2R@ H) #(/Adu `"DgZ}ܖa&g&:l)n(8#:ogy[֫\̈́=ێ6yOH) RP@P)v,(6XuowW`;sܱ oZ`8W\@trv]Ge m:;!-V) R@ Hk) j1q΃VgO9xAǠ6ҡ>/Y ~َXA֥߀.!d]z :.u9xtY !R@ z4?G[2<6km65S%+v,o_֒gXcWgD :+?$:a[ѽo${RE.R@ ܲ[;/? ./,Yk◦\z^uY_siޮ7 oyt0Ftv]NݞH) RT@P[\uML{f ު@ǰ]G;Wl)i3qİ܀.rwCCwD-}PJ|zR@ H) ^qzCnIk>!Ym[J@Ǡ$uAZyǝ܎~SAg?Uܿ@}>Ms x$z8;rߝc0l9Dg[ӣd9n]@_0GAݶϒJK) R`n߮_;v0\3@Wgyl!β:{fk ҧYK57:A7ܮη.2K9Irc~K:H6(sK5R@ H)0V@P' Ϻ/<63Ȍ9seW9+lpKb J^߲s/.X6;g/ ނ)Ǯ :\4S~{}߰-S) R@ tuTR~A%M!2}r ChAgW ,D8%.a-ַݡpZeHta}K : w79V, Z'R@ H) @[A][*+PXP(5+6ۼnLixk?_nҹ$:omZ0ۮC]ȷ j(M@t3/Yduu!ٹD-) R@ P@PC4US ou͘-[vYf3c0@ZL˾pAFoLKRtV8[ϡesǗ Р,]ʓ<-A]~uE H) ( D^7o@YR N WIA e C$~:4 :>U{#R@ HN6f[ΰZ\ǡ)GBL9ÄLfgo-Aw{o*pDK0 8˜xQZ!?aՓqoqD):DPVy) R@ ؠ-dHk.$an +vhmK^͛ CU ]-@ձ ߢ  nfб)u,,!uncõ.Â( .R@ H) ^u{{e(ŖnQ0M.=s- :ߝAt9Κ(y:r=:s/(3;\O P B H) x ^;gR %al W/a0*٠#/8 .o@7$ЦcKn@Y.p]ȷDpd9um\G ˬU=) R@ ) 0PNPᒕ\e,iڟm0g~!%1 -24LX2o, D :4;#zLLqn*)V Ё@h_'WR@ H) ZAC+;գmZ|$Kx\!sxޮ:‚(s~Y JXaK; )ީ ls7ktM[r' Lt(9̷[ dر@P> W H) x ^];&x޶{9κ 3e*2-̆! y\" Y'3h CDuvϚ˓ɸ5l:]ޥ l@m7o:_R@ H)  dm)N &PbA z.``Bly,wg @tAt3D7ۀC7[NurFt~A!YS5ׅ LAtR@ H)p]uVoW,-ȃo.bIdNڜ qfD7̷c>C#хQ=i˰lA ^}8nxbcGRW"R@ H) nOAݓG 2V؎; ,;]Ye6=Q S.-V؀bMt3*֛.r֥^8a~Ap|eɩ[~T@ H) mN uYvM:7!sżK\KAGDGM tw& бp؉{mOܺ#Pg+X1)|h$R@ H) )=_u xà qk\2<)W ]*99,qᰗB]9̠  Y% :; x8x;l)Q xt!ZCt,fY4;gt-Ň7ooླྀٓ0.yrTW H) S9u-}zd&J'2YdZ)v1Y7xIw} JH6pΏ+xtQ5ÔKsb-*\2o@g- LРkzR;rD>pJ) &4 3f0r&}9"7fՖS [! s@tfʅt;NҠ Pp&ѝ߽r$praYt5ыb5[dyrw;ꪊR@ H) &m-WGcٟ#GmYŗyɓY1Oz㜽;g?_S渙ή-C]cq.<@ł(,I[ϡ߀x@e>Z@e9ٶ "N9u EqK) 7nȵÙm@Y+dWk.e)A_2^e_F(zVCzX_Sw4fD7\% m3nΡiϷ,ַql])S?jO H) xu (%r.W7أ_ˢqh!/2*kԝ2fMg="2p;r#n ǒi:Ds9t\2]Aa%l ʎ,Pe~!֗iؤR@ H)` L'm8Ϡ7N# l9K.W%䰬aeX2pDyHt !]?o9XDF))t|j~3׵~Cs) RE)u;m0;|w0۟M7O'[y"AgItaA?]sVo@7c˥Pg>^eʩ{f_C W H) 1u7vC -3 q弄 \l9\u7HΑB~uYָ\^͒wc9AE.l@0\n@gehaMK4BwnHtVD7 \dp:Dg8vs<љ#.r@-Y7Cˍ~R@ H) rnrA<-Vi/ae\ 7[tfltH[|@v̔{ַth-m:r S7߶ά[f`|^[d?9uGTeR@ H)Sw[Oܹ6 &-ϦHt;:|h-rX7s33GQЂq`Kqߴ6HtVݰ8<:fieQ.eTn5B MU.J) RU) ۍ|MAhs/t4%\uétH ްYW|e߃.̠J^% \&畩KOo󰤻ekUR@ H) +W@Pd9?٬K(avϖpU~>a~tָ':XEb`BϷdL-ܹkA"`Lt>9;fDGnHt4.#$:F[pgcpúquOUR@ H) =[4E!\b5$4>FQB^wQ,̝+9 >stޝm)!`\[¦\-DP0q.Xs0^lYb!J*OG\.`r;e)6ۤR@ H) =w,`X=_synе/`B#Cd9%.rq.[DO< WCD-~ W{k 闸e8}e( uFJ6,יH¸쥠ԟR@ H)"PFo9dž O m8hJ1-3|E :3сHtrv$&gF2a}˚D\/t~A0lF:{˨ |zR@ H) KT@N#U[ZgGhH}3|6vy.*)!̍Jg%Iqzf 2)c-_cηXlo@gZ9[Ћ# :m`v;fd̄.R9N%/1r̺R@ H)  ]Z#;g3 ͗Nc<~83(r /؆ HqCnG% D}n&:0_n!vHd̺\}_[3R@ H) X* [JtQo ZUW%(T9O[F6B%.Gggˢx~c:;t9 +wK+%^r%.t xmvg+d3ͪ)eN6u~Am]֤R@ HWn_%1[(+xʲ}3[d/UNtaE -`ICK*HbH 6_2Xw^{\pg KC,t3T&4elN!ԮR@ H)B)׿\ŚKYX_c `6cQ(fQ poq DP': 3q [qlFp :AD!Ł‚( DD/X_$a6_Wn|kݭw GUR@ H)R]2ߒb!K: LfBZ̈́Kp^87\Ksvѧ\uv  8;nu9KŴޣCe^]ѦmW !~;uDm^ exYTF H) VAn= :2RMJ1ytIDwږ\g-xc yZev5wK\”#9<7- ȅAÂ(Bg;noޣ hy~ !2Oc-ù!ѡ@c[(f*ZBep;[֏R@ H) @_A]_$g-An !mu-\B_-$dsp,Ys:L`AwA˃e sAKWAg($:{w[t8WCɳ"fkm1*3:H"R@ Hפһf'˱c?2oI2ZٕQ 8%.g-˼$sg{NK\rtotX~~ DMgOl}:Ԝ= f6KnM奀R@ H)j9Zsvy>լͺ0/tl}-j̷,&yЦcvɖVϠC@t~A!!2\Ц9rc #5xװx R@ H) ^7:l)VQf Nz;#zefpS[Ҡ;\ c\3.|u̷d #ynFthd]8gIt4fLbYyt#)p 1q+ЯjR@ H) x 6?4f5 QAg a7\*&s%vW~Gm99ߒ\G7.[z dGCŽ.m:]DK͎ˠj:6q*\.bQKR@ H) x6 ݪoYkw>]x1]1u}zoi-?쿷QN @tK^4x|KQD-=xwHtX2,kiW@t3S+BXR'+.aCtG|Vr. jSVX `nVRi) R+ >~ANBI "a;H~΁ x@? ׏vߣ|`ob A7>ǖc8  /2 岖`.).6la(ө잒wI+R@ Hgn}(ff]rT.,ØsAwƹaS+pݮtppÈ˷<9lZ :Dg|K[!0C0ei>o-tBanNŤR@ H)-ϷDѼb>иmzˎ˱6=9tV`}ۉa?b: )v=J-= e&;-(a9oa𰎸J :?n, Y+9ࢦ}HP5㴦|ua>>1UK H) V7w:6̀pm] u>Bo١:aM- g9DwK4;8\G?Խ f/Q9 [x>܀`; sf ϐ踥x-ʌ^sD&6/ X 5Xh|P#BYC.J) R`n`PF2g`>kNfnA{=QPwAA{-98GS:R^ ,R(x鸍8WvޣY QVuX]e֦DĶ)VWsW^&v.?]"R@ HLހ溇>Dgoxv| ?+k|7w׾};up̦?uEs'Q[ܹOoHwb;Ν @Dtm$:gE?3a;@f#,(;~{wvj.uC\7?EAR@ H)М;Mf[v6>[ܴL8+a|98 +8!Y vngDwpΎ' #%՟c>pyf.WM XfP0oY].,bw$YN2?O[6^N;>x۬{zZ H) R$-:(x{uC.\NSbkxDX̝;u~8:tvG}?1wSc6]0raKu |vs f ,bt g0[U3U3‚F|w1xTs*&R@ HW{p˳͕I,ݐff?\įBohY{~᰺_?s>P>K;u@;!-tlq C܀G?|%p,GCkX 3Q$Cf^P7\찜4Ko I|ǥ @%G≮HoI) R@  jcer< < LhѱN0;_[_Cv,+|͸.m{Lu源r Pw^dr*Q+њ:Z[ƹ`2YswCy.|(дH'xR@ H) QB7$Cz S|&WR{%s{_Hcx(h vvqHt'ǸcgnX`YkCfeX A#exXa5nJSgYCWcI8:^6=R@ H)  F[.[쬍:':}=зwv/1<"._ I&ILQR_˰}PF+⿝;R@ H)W%;u9t'< h٩&Jn":z?9M889Gwc_63C;9V7<\%?D6~*^dKdK+1 {:R 8W6{ŚU@A5mZآ/%R@ H)xP x̳~'.I7W^ɫ`5pʷ{wّ.۹Fnvַ4wػ@;\1 g'?vX缅%ή[:;;p}uv=[5QlqK{ uPh>ہOe!ozsD>vqsSZs&K) R+]a 7x}/,S7] # )DaKUfKץR@ H) xQsyh$LPEdn)v!2<^pvQ\cR@ H) kSYBq. Pk}=Ľ/B "~_k8ufu]ro[pqlq4.-ܳpːoc `o̺@2Wf8#Zt` GAܝnvu] H) PA-1ʓ\$TQ9,3qf3 @h3¶y;߁ y}~&MNݝMw\r8 :bb,ԡpʷ΅.YcС̏^b<3lXŜ8{f Y^+EV>l$;u!˩oޕR@ H) )tnxZMtMkj⥝qK۸ Kû_Fw.yoytXC/i8gqQjhv~?F}*;C s?q;ΣCf߫ͰD1LgU?*:\Ê5Ir`މntAF€nvBR@ H)xN]>H,nm1 ^r{5ߌ/C:E8ďv\k2-uaG}B^Kb6j(GG6{-=f ٕyWqn@7fFYN,X84yoVkdbԹ\8$h^rf,jgu_]) R@ < \jx˹yơl2V1 Y':+w,@w;./vBajj %qR F2zygoȫւnW) R+pP/ٲ\ $de+sbY̐îbvk ӷhwewuaݏ?: N] :ANPg&/B!x}0j_7ߗ͠nb{,JTsuŗޒR@ H) ΩA7Ƌa?y98\Ǒa]H'g|زO C.0/s 0MXǏa'|!Dg/\Ge6]1w]AtT&፮18ѡ3dw$t`*2YE\Q-?)9m26Y'y޾ؖR@ H) ^uXcqC( lƅI [p l!\t]v| _[%,wR(&YوN7Ith\g8G3x3l:Ω?[ȑ`:#Բgu K)?.ú0fb0cqy H H;w_%R@ H)-#-k@8{I/ g% oޗ 7ߜ#R WVqFthe* |16*H/v8l.1T\w-^[ڻgwsK+O|Ǝ- sggP7^gcZKFcR@ H) T:x=foЅ:\٘M~ po\l fmfA9Tufs/+@}[rEU2!w,1߲ƆPQdpGEݡAW?́obIƳR'3k߀j\ H) /@X( Р&}:>ޤOp^j2S/y: BcyܱB~bFƐo ?Fw,M#E|9~|ѣVwh6IC3AB;ixäوrl~ "W3!%R}Sd0JkR@ H) +T: o@0p0njJoy[;@ q M)N%0f[&:?o8{V9p9DzkyЫ4C ecp~w4} qlُ_a !v/S$f纍5) R@ ;uA7@.g力"#V+c# y5 `ě+3>MvX?oSrؠR4ZR@ H) VQXޝ\` 7ܼY7+?l~8B&QiJXd Y!OAAΣ/YG}pGܿnpv9:InCy_ȥA|5M]ݑD87do_]QG[?i1/.;]>vzJJ) ]ǃ:.qMgY͞6@/m=U)a35#M*[JR@ H) sWZPR@t-F DGiYdV[{313';EvA< ]̔A8B|~0N(!N]a%(#y6 vvf`FO,}Ǽ&~/ö}N;]˟l/_Yiߠ!GxM_~BO广(k>*&R@ <;i:@)|$)RE"e&ٍːBs[=N(3솎%bu|謊W[ >:33Dg}f)v4 f [7Z,L?{Pw˟g;x @t?No>?zr8~;:NrsvXxm @ӿ=*6U@ H) #A,,3Oz,sNH >cpx}'yCyCa`!8*xm2%ͩc鹑CĤamb ?C`X.S-ԁOZ?s;3}!~os>9xt}o{Wفsŏub :&ֿA˒uVFhJR@ HWΫND$'zY- YήSY opi[ Β ˮgZAʂPmsg]uZa%h7:Y񊰷_/3#~OFt_o3#vNr3p~~o9o8~΁ExgW;Y`R%`K~wVާ}{!*R@ H) P .R]p0cgv&Kla}7*p]yG kx, Z@ᨽbqɃ x;AGu& ?Evv˷,)3Be:pg$??osp}ٹ :9_~oz/-/k< x+'anڪ%R@ <O u>/ԩg# xrN8̇;K un\Ю㹱cHn=ѐ b; R\&Շ.ax. q%X['iK/o;>[/oo:OwX? ]_ό@;v%ùsv<<ʲ\+vR@ H) x/DŽƐaHQjj@z?9;>q+]o׍7/4xrJnt*/R@ <u~D0!t@j0\ߞ*Rfgb*&p(ml.C )ʻpy)g*(D x ȴ$(PB ja~Dd iyĹ w}fu88gg~qY{7~vvUФ6КѬhŘ*닦R@ H) (h16~"YtM]׺|h'pG'atB EytL~[#T9ӄi<+ò>sh3*4^[vfÁ: uLχwƾc94W~oEf}]:u Γ_{woxl=T%[OՁ:RYv8׹/qΪcix>2T1p,r :13=Fq86}sW7@t,l8g/w8pŶzmkלק$wAUR@ HQiu^|aC;NÉsWMo*QoMUUR@ H)p uX|5 Fwu/E腔[t~h2feWk3Dk[y5fBuG! ՟6~9M|hQcHz~[gi}KkS8ǟOfG`<]1gFtV~?S}eYؖm*,R@ HPQXrJ 6aZx(UXa2Am%¯8giyE|x;OIgUykd-YQ(3%ڡA8ݶՖg,qY`l,}/[?zripXa\ pPKu2W|UR@ H) H~ {b]DzC_em S޲Do$QʘY [ @b/ v쓳e0FzZ|k.O{ߏz';Â({4ӿ=o/GѯaXex͠a<~~Ewעks7j)9|b5+R@ H <}J 2Z(C*8Uگߚco3?<aor.;R) R@ <Ou[崩Zއ TŪ8-u,9`bwIZ^SOT~`{uP53Cu-/bXl99#Z.{2TQ g?t8ԏ8p!;;^NΘg,4>uĦVvRGU^y R]) RxJ㐂Dȫ5 3|."̄|h ,_g]x;k[N'􅇨l ?vM,l&QTz]mqHt~WNpo əs>8g<]O$%hv[m6bR@ H)  uX J{t,:gVSyr9ȇ1'P-*gdS?$ٯn%v4 7ʷFɫ$9T&:+kO0x[uv;iWi_r(l^qHtfO@w3˖Ȕ#tW'aMasæbR@ H) x0zv6}R6ʬ,|Ɍl!ǘIZ#^j#d+%)؋rؚ Bz:M$.Cm%=P O#Y>``X,NΜL;n:P٧Sj%}Jxɖrv`s~ #X~~~)h@uq<{gs/o^8;sUF H) x [2;U6}вnG6X캱hD~;7?<@tfv{ttF05 b. :kP;R@ H) Zqfc> 혵N" wk$Cl+rxZǾ# ,<^?ld|P?Ҍ=90K6Wcs׾oCwm2-!9us{|3pO-S&? [,.j P7K&qC]Ǜ2fKץR@ H'QiP3h@DFŋ3A'faw|`y)ShZWVuWPD)nˮ9^+T`K/00ˎ0˧a;N=;.NȁxvyqzwFtyˆ׏o#.{7ppR@ H) P v/?ӪgS,>s;bON,zVfN(S skHu.D}loi0fP.)C߈Y}L2 ƙGg_hǗrb/( 󐜉#ӝ,mATo8ɬg]`) R@ <OuaL-$^,AWO`V8g8[g4fwnpjүC!_V$<oN( 0l1s$7iqW:O7 98N>{΅86!kv}{a@Gyl~Dů;@1Mk/R@ H) G'<@t929?w^Nwb͚ռ!11%p k,o놩zM$x>b o#,S@77~yAf;:ZC8;XDWT'I./AfؾزmZ&k\R@ H) <=ԑdk;ZFw~}f;[&溽n_W< A xEa=@ѮPG⹎w2E H6s؈jA RЅhdx<͋Y?Wv'{p]&:d:{݇S;rs>,*C Q}crkGR@ H)T <1yR!$}YkD$ 5KMneVdH Iģ`],+è;cޑm.9#rH}fAX7\Awh䭟f;'o{':;g # . e&iJ'qUE H) xP)q!d Fg鴘J3W1en*$Z>i|i$mu:f3g/M?9Ü_X>s؇W~KlN>?;'h \w?2; 0͍.?:P4iuϣH) SB-")F^։jܺ2%LZk3$ qSrKPBҪKVZUK H) xB {;<  s5eC-޾NpS"VSR@ H) (x] ,~lwt9 aPu"$ cƮqL%#}ѝFRl;!ȝ6ySzlz30OZȜښb^pǠǻft>Mv|9v:OgxIr#ʃSw(%@3Y=J.}֦66} TQ H) Ou5(9̐*fޱ$>Cּ5Wg#wd`\ xhAYa3C9~eO䦉y>b6]AthiAoﲃ{'; pu|ܧ'81tK;,/v#bw30]6 SR@ H)8 <%=Kr@/4mʵ=ء5쬡+wE&ʹzR3oJ~=yg2)jf5'RixS/zǯْr/xϲΌ?nGh>0É#M :nphEf?xR@ H) xb#Kx[N/2JB-my˟  cfu#AZ\aP2cY5ԖMv3. Ɲ;Ԏ[23'm0|_eOn5H$:|UF H) (p[P9 ?-Ne S+ ˃ #A˛82sV`%I  rzJnyG)N{! nuqeixt n]ӲC˜ P)1 YqB/yNTW H) *pP,R(~SlLʙal{I!BfTK H) A':[3{J|Y^[DZ;/xoQaQz_Ҋ,.jaPacyڋx =tyK^Qd9n8nkk!ln}f _޳|7]3sFt)sK0C&L9 2pۚjksҭR@ H) )Pt7$qB!zl1gS%Q1Dŗns[AޠOj%X!i;x1cc6ƅ+gh.,}m"j]rZM [ @;R@ H) %  3'8ݮPO*p$L d84 cX&”u~/  rD5KYV0ϧkZEoXHZ)&a*syjq6uMb3q w qyjA H) A:P*3[Q$Sk^ X33]"b<`C¤D';}D^ ̼nJƹ0ތy%mEY R(ii6& {ؖ5]Nwæ. sDf8M9rDtAeR@ HS&A@#ُ0ыEV1)`ά[Bef<%3~x,7QUۄ!q7ےEY2*8|t;}&O4v4;sO?zZ^,MɷHtt>zxK}[]{g-ޚHɖ)!e* R@ HSΏmB!'pL!1[wӅͮF 4y u UE* c {!1s3SnH&L߅+C~Pe'[ ܊F OlyGPoE+ұGMqTL H) Cn O/6e RqCrMGlÈNQDwaG&ftQ`;G>yON0N ̃.>p\7E>4kn85?ƿ$Y>!Pq,8+6Gu ٲI) Rn9V$%qQ:Q!&&lYEԝ})27.t@Ax MZ|Y Q/OV(n./ԂR@ H) %p:m-@[sr)}aGhiND`ڑ|;с|%26Y&P\TalYCg+p jn S(MD ~>[@/pXn(N`)ȞBLeߝM%H[.8pݱ̥ꦰUX H) Kn晈$6u![c1ӹޞ@qy upz!.w'g!&! :eDʌp Wb&J5W.}Θ=}@Z0Jt ̇xM) RP&.wi mF=<n&[Yqa6:tG6)TXv9u]p\Ǜ$L4޸\>e)ay c-:˜8/2 @wx >mȇXifUK H) x 2!we erp+}0Wr}got6"\k_y]-RlY?u\BR@ H)noO ԳȆrφVˢl:Z3tAfm=^NG,&c L a]|$ܚiB$Ĺz Kn݊w7rU̦[u>& ˷ p>;*87,o!iZR@ H) ncMX~hN dIJIf2pA jca=Y[V؁p`mSzgKKN^yq>ہIy]ߦ뗼H cgX«A J) R;.y@#d̼VFy4#1v,Z^r`:Nc95'R}Xݰd+O}i"ϗN#uLa̻+vWpG!ÅYeR@ H)cL=j* @ e>&S(gDoa`;*^ΒYrh͘0Zz˷Cˍoo!K6rt 1c+rl:,t7\waQ10pBP(aKs VXnGC奀R@ HGPච :O;,7܂)fIf`!9PI!8K 402R rn%-3&ļMlۀY#l\ ؟7,KAӲ#%#`ׯh=u/=Z@pWӑH) R mm%x8)3-eC?wbHw NCn 9l*.Ėي׷ˮfa``~3T6Ht;r/TL&zK<:/=]&]KI#R@ <7u\A8`N1dX@jzC!|0zt̺v E/Jg s&gs_ fBMDQ!UI) R@  ܰ 6fS|eGn ->=3|x&rX qo?cgq!vpv/C˜GGkV"^B*f]anۍ4 |хvzGT^ H) ا A]1,q6Z750rm::ജlFT \7`y|H#Ir08F%ˇ;q&vVԬL FWs~t@q7մsE@.+UR@ H) @S[p@j1b`]deف<׭(B*q<Јr $zjn&I)mi?pؙ}ׄI晴xq8.ߔw;~q(!^!zn,omR@ H) @P!z+n[ʖg6H~˹M)vzfe yyu偖W(,; ^40^E1& [5_ٰ\FǦcweg|ff7̎m.BDq 7ŖCUnUQ H) ؤAC{ G5#pA`ea 8Qo6.>/̀L~Pg?u>"~sx];,<ݬ&gF^N8q›s`m߅u#PVa*>m c^M*,R@ rnx?ڿGdΚ%p]?+ɖ8l|b| .-oLxMD٤/$ek+U^I/w˰nTQ H) Knl#YEj &Kż?9̊ 2Õ6mf58MGI1XY"/@zv>iHy:T`'dzk/$n[[]1tTu+n ˳=hUE) Re+pPQm0O7C#%@7<[}aY2 Su[1N ˒sy?}W|t/) PTL&O]ꐱ5yϻg fu=-yܢ`f5B*2!]h/ëG;tMyUL H) R [Zm]Lr;f3+i%g?QJf:_[ 0L/ )C UPKYl9߈p,,b !- 9Ǿ<,;=7Ceh/=FgZb0 oc34*V al+k5#)F\fx B[͟]R@ H) 70; Ԅ`f(3P;6Ok]z7C_cT#̙.yGI=KA(Lż0o9/[LYJ\$Τ>sp4붢Zg Vf74p)q LŤR@ H)`  `~ C*;z]îuyDQ|y`OAU/g'L7< \|ağ}8!G͒'udG, a<٦ 9p74!pyY$~GEIUFFR@ HB]e=5H)Z2LuB/-b5KJ=^ t>➱=gzqk5N]ر2 ?/0?ƅR .xItwXg{})xO/oax s =mg Xr ongA奀R@ H)Q  &sȌsQ`RH$46-;v~Ȯ ^٘@^߲9ޚ1W&Z2grh }\ Dgj'-4ѱf|y[,'Ĺp>0f?^EM̸hĢnuDVa) R`7 u6R`:۟!bNGD1ae-HyS3dPt ; Aa\ԟc u<͗hWl,>Ԓ0k20q%ElfX Q%R@ HM "Y6g#u+X>R-g'[^Z>=14i倩6cdt`녃kByMϳ- ʇd&P Z7ܛ-ޛݾ⭎Mǜ"yrH5r2끚e_Km ɼp Vp.R@ xnBB`.O>-0gU)rnHMdpj\vKNT eD Os;J,=Iߦ;`sǿl!b'[EY/ڕ-*F F χBZmF72TL H)  u py[u#,Ӵ̨ovt^95 >ͣ CfB;s Mh^f-T7Sg3xf|X{PmFt>׽Kf!r?z s>ꗏCkhsS[IB}vR@ H)U[`%12RZvKe'k0ǼM?.+A&乗)'gc3ϸgn_ZLb)Hf߯9Y-sݬ_`8g'?ԸF5ѱz.|Z`붆 m^6~q V įiuTE) RPডn ali-/htvN[w@Yl6^gֺ|:%ulAڕ3skmgyXUMvS"2 m}k\3i-\AOԮsd9 9&$omXR@ H) n ~ B|RcxfC:%?D#H.^\1.,? Bnh񐣂6̷} oc}N+'tؼcK٤kzPfy/\Ow,gM Ǹ)GK) /X[:4.#RbRYpiMNС,Uk]ϴtMtzy]5^}pXXv}73#{F6y9mFx&[=.?eO[`mr2aȹ\/\K) T: *{lgo-9]Y<ޠ#Z<吙$%J-UETu;a ,_ޣ,7tv;Z!maӑ$gHdrwJngkbӭDۜɡ ~PͮQhZ]CR@ H) @S:|$*xI M;vw9 !,y>w޾9{f+rv[-2GD QM+T(M|g\޺ .Jn.ٔkp^xit RfK&#?dR@ H) ^udû [5~+Γ1 _ v?i|; iMX û`Nk}X;YM,- $Z0}+OF~Jp\KVuLnaUڤ OP.jNEeR@ H)W9AG L# 줡΁fynX;oO;L: 닐<ڹ*+j* |TdN+Ejn>(\3ތ(6KYVѡr`- 4](f x{20/|) R+pP0߹cC@zEOD#a J\!gLGGlO`ۉadQFcp&}Mn跧[V/5sy_/B\4r;-m.: lEvHUR@ H)Qv{ZHN^$xh!}y@!ׅFTXmD;,y&Ü= jiV |Π0i3.x'.56sxK4=7TY."G kבk]1/l[fy^i箩R@ H) CǞp2ѝfSU=5e,$"q1Ce7|kpؾ8S7Ü7ʛ}D/_-ojU7Ih %ny1 _yznZ. tzJ#TF H) A[:EZ,v~m6C'UCe)gvfc3X,eʥ췴mlvaؓג-BncP"R@ HguXvӿČuy鉙M<;`'=0#ygr~ae$AlKz1{nny-nHkWڠj`!ay@bU)F3\gc.'ue ?\yXrCZK) Mv `(nlW(5<{jtFЅ;hCOD<Dlp :b^6fa{ vEsaDuʃZvxfw(^׬pApl=#۷fhiU$a> pr NE2w 0x_S9\GRR@ H) )ӾS6t 133əY]X.Y:0R m#*eM:p(BWA#uvkw$ffbIgk!Vupr*f{ kӄfZ2&.B*!ᐭ2|'Hu_;bR@ H) (n+qe\xBKCr0K'UIVAu߬@MŨd /w30k\vU<އ:;9Ή2-&#PJx*uovP* R@ x6P+̺$~{Vf&LE֍ +޶Y+hƇh."D/"sAlK8sgwAYG,\94̌ S7.t$uM< ֩EN\5ϩYv^PEP׼*&R@ txP mi xG0mN gXt| 1Bog#:- Agl 쮄`M }aHv y`yʺQ@D3UCWY eG#;t3Á>` aWS鬼Di-fSV |t#TT) R@ :-fX]]j%Ĺ%=O@8HKC[-EX ^є] pn{n͛ ֟,wů sp͌p/s;ӝ2AwO,=.Aq&ZxS+>jJ H) x :DLEz9{X?yUg0۞aH> ѱFKVfiGTlNw ovʣL,g ](WN\caB\Vr+n5_p:W3MlUX H) Au' z1{45ĘHɔ:;຀v>XCMfF%eu(]dHh;ݴ ._o숙U +H=KΟG s_Ym0&pÖU Nu~AeBqR@ H) xPG0 p8s3wr~ZvsR69)?E&:k<4Eb,M g(L.ngzO7،Yqάx, &uD\-}}%*C*@{jA H) xa <'9nɍ 8ݹ-;Yϐ%8zDR*zD82$xoތX=HD epXvW=~㗝4rdƹ@t17;dWw@>e07YWG J) /[u@xK }vvL6_%p'|BigRH48sXk X$dz\̬%U\&d c*WƆ8$:6˜8#:uW=Wm7xvOofÍ۽42=#]_R@ HT@(k&wc{FXzs*~ -x,/z =47\fיb1$dc%wxw/rQ)퐋2ݩ+}.鈑rю1{DXt]S) R5+ 1̽Kwc [Xd [>W3 4DbF"pafEM/L.d`X 7PB,2Xݳ$b ~ٕeGl#Y'A4 0R)) R@ <'w!'FҔUslOGCqiy @ %z'I/2/uaSj5 B û(r1϶ lhMӬrV,7H#QWs|̅VBw˘U@ H) C ;-[zwĪ䞩xkH5Շ3 hZ8f :B`M23FpTFǂ6EQ:LnU _y. `6B6:6N|&DWgN[Yz7PWD2 Y_9;4*DL,bȬ8) 1oASޥR@ lYe''IwC'cwg*c͔Q?bou?zQY6؜`{&o9[@%\7'Gv, s 6-&#(cEuI &U&MUR@ H xfPׄZJ(Fց<<6A{>M!'#Øc[2;EGa*~9C!Z9s),Z} =QڰYiO@#؁s'kSkG>f<ɦ*UR@ H) jMl :TNV[N׀GZBWYp*C6 E&ƋV5tɖKx_f9axMJso~VSKPoFttܼ;*&R@ x@4Y.'jcXrpx*甍1dc[etצ;i_aL| :c Z 6#GN zI_h&zXaBnY 6zJҬ#ɮ|EI0g7@@T/+#z7U^ H) ء3: S,'\ )~\sKlsgl2;8)+βIQk /HJ əEtv\~߸&^i. cLo6^u\ s dq O㌈Sv|[ ,l>/sڑR@ H) ՐCHi8 ʼnp\37P"Θ??`͈+(@* D]GWD.eXlC 'b kHq` Y%:_ʒ]M뮵A(9O[]2©QR@ Hx P2Kx;HoR<סX'*NN D!S7+FycI_,uC[zqAmi w&ǡ)܋u[_e[=mļfl~ҝHqTRŤR@ Hk),.91¡BZ Ћ^2xG*.wшl_c?SJ3F xؼeΊݭ1s~p~,n%2(4Ӗ@,vɬa$돌- gM1ͦ,[#ם0n[AǦ<סzN<^ P <7yZr4R(@kwoc簱MRRΨSe:hDD9t^-lcyhzϔ5+!2iNk*#R@ H+,Ϋ,;_ [<ɬ2ݿ#ۃ.{t˼P_933yB/@9?`{T)`EYd)lGrff!fg]-B׎n[?BD2'o]Esk_8d鎶ޅR@ H) \guIf&:n:mxݷI}N#\Y&  [B8, Sp]@'9ya@zw,M~>>0َ$]"SuYl1.(9x'~WR@ H) +Q@#p_; Ц*~HBcѻ#ur9LS2k?aG;%|ք?c5i0ޠY2c"rҗKD =ـ_cbT3ki@86P uv#%O^-sS [Z:caJ)  @.nAi<:ڌ;8Brܸf}-}ZRϽ2=.~QLtWYW).\ː~?u0oda_w9,?,$f~9-GR@ H) *^pBO?;̣ f4H x-(7dHO:uax9ڬ ]RR!1|G}:(9ˮڛ_s_=7pB$x/|vnyT@ H) 76,0L;ݾ!,ae ,sk;<,kvFŪMT[Ω`g2-':\3 ߅GIpݓ]~sJ{5Fn3R@ H)\,yI!ՇMC u0/wn{\2@찵YGz)Og-S6%av07 :֮;7oאvfY<6N:7]f*&R@ N+n2;Y01NHYC Ù2'Y$y }\aOr? qwGtfeӶGxBqfق6 ;zbkDR@ H)CguKFrxo\(yFˡl¶mΨ/< $,ܭc] ޲s]x{4 QX0(>Ms|?ӇUf?qv=KdYų_MttؖmlvTql)b* R@ H(nI<#߰pǶ# 0s?Sjb~Hz|-xs=3ATa$~귆cCi/]Hu_o\UiM̹e0f얨3lgd9s-t8pg^hn\DPy) R^>euCaj["8)v8M}87ޣ;W.Y䫧x6Cu.yC]CuDH+9uuSZfn?l@8 g}OM UtvuvѲ{gavh'|w2{qP*\e`Uw|{LD~9~GJJ%R@ H+*.F b7=+~r'?!CkyUr"#ZrZ ɣ2eءݬ%y"| h7v?o}W8}?V Oy8/:df?vpǀ'One<  chxP,\jOn K) RC]9^s_% Wfr//VNnd/ ͗rjrusmT>g Yh[jB/vC*7|iNW~x:4k\qﻟ\a)40w`.̷C SC|(ԦR@ H)TCYNd'kΑTSnfClD87old dxlfm\|Iƽ"kv8_IcSg}TA}ا\;v囎{/=?pjrvWLSY a $UYrDtoTL H) x ^ԁx|CC!Jg36ϓٛ6qA+{e(S|C.s:_%ʠfcb K,,i/n~ǿW|p-k\2O}ͳu59p:H~ c3sƛc~UQ H) iA~.Lk^8ԴA$ x:~ΐ`$G5a >AbǼ%Y9<^>\?;pCum]}딤Cxɧqf-ZuqBhgˎ'VKe 7Esl#Mm^Ѩw W:1 9y#TL H) xh^ԑ 9;X2̘ _ss&}"i1QxrDMkspY)=A;wٱ0gvyu\u^\ōxpLYvMDx;Dն )佂E%=厛*R@ H) C(nKoT.o!9(B|N%d2osБ @uӺ,0ې<ə~YGt00?hZyy' [!vǚ~^לtG݁~;dc|Z`43:hWpZ'saH5I) ˁ$k4;H`ƻP1I36 y.8I\a&C25oOw~گ>(kka1뚟MuOܸ9;;#VY76G=+l v5˭DP݅wVեR@ H+ 7@&%Y%2(ЌƊ8C $ +0C_-ϽXߠ}y+`Oa?DMU5meߒYHR@ H qfۣݒI TCxKˣU\jiʥ_gm2i$7a`BF۹56gx:b0ū<uc٬kw3@tS+aLJDnj$#[W ✈("|vԈR@ H)p]^yNQGefU [e|!ї:ą;\NRL88y7Y0&g!fbs+sˎZUXYp%`^>lz[3f1V?oVk+…hEtۧbR@ H) (.Trl<'9H@M9# c/8)%;gb"?Fɑ MecP`yK%]?}5·|h5:_pO}ayLua1{ a5}{|&OIg2;+O]rgUW H) xP^ fW@=f֟o\Dw<{#[C D_.XsGS2yE`0.A7j-)&ʒv/_w̠ pX@|ayL0!IG1 6McKw(.oJ1U(w1Dw .DQ $]Mnfan;;Gn*"q ]vVZrTa) Rx P}Ai2]x,vb ?R`dYT`6 W\%V%X8=V<ݏ*J) KZ&~F:nج:;fr#CbG:aT-Z|޸ q,K9 `Q,5ˇ0(EB lWwgh s/QjK1Dw\ѻCfx-JP`+[עlzY4s ]]WR@ HTB]6 <Va X]!Az =!x4m}r&x[Ʈ: ,{XsK_cDMS1w/f2-qݿ3+gf-N*&^\]J];f k7+R@ H) S)b 1TYIzg+Z:o֊BR@ H)p ;*̾5_aqaښƵ C jݼ'sw\™_7>g tњ`ycp.\Cr K Ur>)v6"f;s!ǒG`fZf&Vq`̷U"MCR@ H) nJuq $~}Cn-%<(-fu4oVЈHˎh7U|w5t[֖;oY D :sHt0f a\6hKϊP7b~y}!y|fKo9f_*&R@ HW5BT&6sCNfɜmǏ]1tfc2f.n㬿<,w{t[++Zzt>-CGV 'Α r0lY|rZ~]X$gcΖ,3}g6=?̖sqss[UF H) q^- M:&x;iS`0E(Yd<nQ-N9:v)`T ]01X2 &,_ w8 oIkDGOkC'K,uÌGM8ipipDt~T] H) ^ՂΌ)l /,Go[h- etm3D֢σt@Ș8wXw߱eȡ= 5Qŧ\P7%<:sv[rn64~ֱ,!Y(%>C%F)ucvYYʄ]ry8s"2CK߯3htK㤻D)t!dy/k"R@ H/ lGgE Cc:eig f_g3wsNi74-C5[C "NZׄ:T@6 E"'{ 8.-Itt>J>xouCtw9>c#`=2SR@ H) xL^,yS([pItz{ois` 1B ۆw ^0C>\&HV;?] u0ri :H{Q>>엾+~|r]flR+7WR@ HgCعi($NCDDP9u;̶sŬb-t]0q0Q7J.?0lmF@xh~Iۻ|)[ З KԭsBXvpM@tȷ䏹sd~҇6"}'/2f΁pcN&~6t04u) R(213l`a)K{9K\ztP;Dْ*C=CH) /fnj!qe~bUuҜ15X23 `F-BaQcxl>uǮv5VOaN33t . Oli}!יSgt[>Whg@f%F?BR@ H) xh^9 evq4$:oR6};`$Μi._Q8P62g{s{h~ilj8.DκDG ;y3^(SOYwxC̤;aO;ѻuTzq/R@ r^>8{}*u̷d12{dk*!=m}[BuNfBp6kIio gsp}KwY.,yo+Σ=uKjqL+M×R@ H)xPy\2z_|wyH`*C W7N1w u ,<|ءdxkhjb#uf9%w䴁.CJ\ߘʸ8\L0Κ"p~¥Sf\giV %onayt0W H) 0^&ԅ]3A\ DV̤S^p2``fyPPC^tM_Dm/#P 0~9:OtV / 0aqK{h+ެ_9~F!) R@ \Euq̓SLC)̺, /&%z;u} Qɉxf7#[$N9CUH&fk ֛w~]ѠlM|t7:1ӝG;+2Q1vJʿjD H) x F*iCuE&!'3/-PAA]uøφU:Mi-{thu˒õ4$:K\-8ִ{v/&~wE%wS3GHaK) +._Z aF C Or g.b=־ְ v\1Ív!Os#O =ZBaK#%wX Ww&:[z .N-N\@K'ݽ (2R@ H) "rːaDX"\cTXuCD ,0܁R=CB$IDATMLd-f3B)h-L+fۈt-pX8v`q:0pT&]oّhͱytALR@ H) ^/֩ÍZ|O&&pv3S"CNGfsY/Py> L|uK98r]g;x^$#$5#0Yv&, B_vLcAWpD": hUP9Zn׋3]̫R@ H)xNݡwysFC:f-ap+)LEև:46I*,^ R rW״ ¹s6~Y'%SH) (¡nW~-Go~Rq0 Ym~a {p+ѠyKb:,kq8'(R@ H) R uˑ҂oR#Pu!΅yNtŤR@ H)  ItC2q]'\ ``*jbupCm֙NSf@1\pRs`8gGh闞lD26/R@ H) xd^5="8NF?0MT4_F8 a!m)_fK'v"|SMÏvC_E8GN H) R`2͔a5N.Pf]'‚iF;ĹLHt!BV`α?(du*7K\DR@ H) xd^,# V`yrKXLaIcUm2u6dQܹ@tDEOµg8G \yĹeÎɠ{!u'R@ H)pLW_:0c~pK9 s`!H\wmWl!⇇,6{j;C@{ccXhAKPTW H) RxPg{ 8+)Bd{ \Q(;݊ w똙M}Yx}6p՟v${g&-@APgsQ ,:`aׁz UHI=q(l@h_Ç>f~RxKHR@ H) *3El:7;d(BJBYb$q'0Ct+iq԰(z1}7U ,v7|_b/ͦ_'M_e/_,-IŤR@ H) nSu>oT=CWf7249d A}/8vp -5 "`D_G@t¹3bR@ H) J<!4œ` LtV.AC0=m*0?VPnBF:q+lm"Py) R@ H낺O>x~oSi& 4KAٌL:?r4.q6 WR@ H) 7n(V  Ê,]BM-x*jISM0ay_ؓפB9uzV᭲bFtV-=*-R@ H)xKjHH.7B;5PlF]DA4`3Z 2ndlzңȇ7QsFRR@ H) x]P"iͱ ǒY<&-qr '; yShU04oм|룿[/sTA H) R+ns 5cpX]NJk= E;lh$zv">C;'j~ĄsMTL H) R)n+n|./LzaL! IHwЩ;ǿy>M•o4") wP@R }LIsjHWcxR`P|>R"tb])HAuly .3'O-\2ԥm:\_}p,; @#~9DdFken6ytLH*VݷŸ`N-x^2j%Vf˦7U)rvG @ "0PninWHrpJ-*%z?, uUث٢{͋DW4֕\XοT'$@X"0b*Ƨ { 2]Ltwd1fvދ3 yC\٬ x!-,ho[3|C 0pި.dukS7|2IdYfjYK+Qm0=R{ڼ˫t\"M*  @,\>Yl9%") 96?%/T,(;%fc1vail<^|zps @F=-RV$v[x Ưo]s(ͪ]rE3oQ5'ZNn49 @ㆺ"t]Ewn,x縒6zLMt_z]k.ź(xٜ3լˡ.ƾuRer@EHK2d @G uSHL&˹]L2ےt,%)qUdn4rƒˢ Y,ьHs^N @bI)%mղ)E+-)ӥ_p^o~1 ҉{MW.^J[ @rAC]d:G2]&Ög(gb,a/`[sewDչxV,ӥ8'-: @$ >Z{%&{J:raT{(]+]ϭҩ弗 zKPbon[\D @`X者~~~?;x&y[NYs5sn/wH17Kl6޳V|nB @ ^˥԰ؔSrUb<[X] v.ֶn/5]&#qbK @x^+CTmk~-Hu,:XC^1Jooυk %|%gZ}5p; ٹ%7c @,S+տ:8"xSWKDIVEVo&s?u1^uƛƕ)5_^ [ ?#@ \D7tB] 6Lɲ{](2,,T'֑| EH+gWt|M+[&6ts?$@ W8G] uskh/wϪ5,d|cA+[%NShc:^5)^9P !ߋAnY{` @+ ue1trRƹRb#,iW6}KYO7%Wslܩ߮1vbN[!t  @gr<etni"AuZiE#pOtƋOKtm;Yİ8cՕa3F @X!šnuv)#5\[\5\}>~=ݗweŜ͊WŹ8wy5Kr*)N[s  @uD^)u+2Lq'4[J]se]WFƳxc͍URFٿY?'m @ '"HA=v75gnR\+]}Xu՘"܆3M @xWfx[&s3ihzvYXtdubꬷ_{P/"YŘ/e @PXky8ėo,ț;C6;0Nu~no|| ofԗ[O @IoٶtEB_'=4Y&@ u:548v[+)oFm).vdja񊫋p_ a @O u)"}nz6Vu;]un/Dm&F+ @,.N  @xPw u{-i @^)0zkڪ½6 @[ |YK_>V'myCi @ mp5n# @P@ @ uǝ[##@ @`nI6D @+ wn @&  @87>i:ǝ##@ @o uP }{ @+e{ @^M @I@{ @ @?P {߼guSGrgz*d:,~ :@ @ԕ)W=mku~.W!@ @`5/?~zGve3 @Z`i˫{lZ~$}Α^ @8P\*<}P`7 @ XL]jYXo\a݁ƭI @=-䅗{^ztN$@ @ o u+SR1s' H @-TnKYm @ 0# mvkuj @o`. ;ֈS=b;Q  @^!4Խa[7g @ @y/7 @>& }ޅ  @ P}?nL @ViSK߻ tnNA @^,F)u>ߗ_dʬU{U_} @&R7I]QAN;ڝo< @"T.LY.hy%.#K.v[0 @؃6nIui`qqn\QJ{n} @ @hy,j4K.v @ 7oL @;B]zu[” @Qc.av:q71 @ pIU;h@ @]ÕȻ0- uo\#@ @`GݝP'ι  @I`Gn6ԉs#ݸJ @N+uw]#ԉsng @FinB87k @|A.Ns  @ @/_!s*.NM , @l(/7 @ p06!@ @PBPm @M@;ڌ @C uCM @ p4h3j< @ % 5K @ͨ @ 0P7t, @G6C @PBPm @M@;ڌ @C uCM @ p4h3j< @ % 5K @ͨ @ 0P7t, @G6C @PBPm @M@;ڌ @C uCM @ p4h3j< @ %?}\5IENDB`PK! 9\ppt/theme/theme1.xmlYOo6w toc'vuرMniXS@I}úa0l+t&[HJH۰ŇD"|#uƣC"$I۫_y$>h{ I37#һ{HLO&n{Rʊa˫<% ̍Wݘj+1ٻ1 jVN5QRL 4i0`R9]&!fmhH)1,Ly+[Wf%kK旭USih])S^ z}4i6NNfKdnu:f+5 Xo۫ހ,otuo@_k7\E&vhQ/ cv+ߨe9 .b,b>4aEf)c MK3vȗ Cj{2bN߼|޼|v㟎<9~,IX^co^?/_䗟?B%z^<{է- ҘHtn0d$ηbaZ^'XsS3 W:ĵ} xsx\hv+Eni^%3IX\L˸xwq7Mt*݈8b3(( {=Ա|:VdHGN42f>pV9tU?$1Mܣ#<@Th_BvpLˊ|押-heJ잨p'o&}qe|֊;/Pucd.K1elfܖaa}I4Ǭ;P` >*D8fi"H\! Wxhؕ="6 3ZlZF~fu-ԙՍh9 `aMD/`u8kpHvbp.H#Iy[ )V+qki,N*k,a{]GK:oO#KtZզ|1o1NR7pI+ad6Y>f+WM:\YX/(ԁTHedCLe!ʿ^6B L Z2_]ѶY)SE ЈMP}*Twjf-Yҕo ΎcF8+:ELpDž $V)Q Ua?SE'pehp+0P*F hLh{Ye_C朥aN~ꀆHP؏T$هdbl$YFDTI\ZG䐰zoPnIV dYBͩ!ks|l2Rn6 MnBĊ]ծ7󽷬Y<+Yi+hei"skAf.xqQc,.~>_):P[|t l iG8AL5m:iw֒4vќ\Hcgvlmǖ<{2Eahdc(>zށ)S|_ZρH~, PK !OttdocProps/thumbnail.jpegJFIFHHICC_PROFILEappl mntrRGB XYZ   acspAPPLappl-appl descodscmxlcprt8wtptrXYZ0gXYZDbXYZXrTRClchad|,bTRClgTRCldescGeneric RGB ProfileGeneric RGB Profilemluc skSK(xhrHR(caES$ptBR&ukUA*frFU(Vaeobecn RGB profilGeneri ki RGB profilPerfil RGB genricPerfil RGB Genrico030;L=89 ?@>D09; RGBProfil gnrique RVBu( RGB r_icϏProfilo RGB genericoGenerisk RGB-profil| RGB \ |Obecn RGB profil RGB Allgemeines RGB-Profilltalnos RGB profilfn RGB cϏeNN, RGB 000000Profil RGB generic  RGBPerfil RGB genricoAlgemeen RGB-profielB#D%L RGB 1H'DGenel RGB ProfiliYleinen RGB-profiiliUniwersalny profil RGB1I89 ?@>D8;L RGBEDA *91JA RGB 'D9'EGeneric RGB ProfileGenerel RGB-beskrivelsetextCopyright 2007 Apple Inc., all rights reserved.XYZ RXYZ tM=XYZ Zus4XYZ (6curvsf32 B&ltExifMM*>F(iNHHCC }!1AQa"q2#BR$3br %&'()*456789:CDEFGHIJSTUVWXYZcdefghijstuvwxyz w!1AQaq"2B #3Rbr $4%&'()*56789:CDEFGHIJSTUVWXYZcdefghijstuvwxyz ?( ( ( ( ( ( ( ( ( ( ( ( ( ( ( ( ( ( ( (|5|]Wy@><#x+޻;>/>ѣ,<L:_}B='➥/OGaUsmu .ARi/81,fO9_?G5?v1C~3񕟁5oþ#3틋L/M&\"r؟b3:|vƶ>g<m{R</v> ҼgwA𾍫ڵ_Rƹs_iSP@P@P@P@P@P@P@ |8qb5-cJatxgB嵞H|w*?ĝ6|es-+?ocwtu4MOǷ_M0<_o{5ƽ IC_~K'Ynخ8xTts4:;O"Ѣ/5O65 \x;׃4'?~,_}z<1f,-4~ ŧ鍤y55.pq7gg5y:k%?|wmc|!iÉ|'V_,|} gP6^ԤjR]kN?Ø솟{ vlmcEx^1|5U|=F_:n^:Wumk8{^i5}/֞%,=v?_ޏOW^ 'Z|W3%ֽš,~^m_ "4_ x1NbW,N;Cԧ<ZK52 O5K83܃?~g>SV^"FںBJxz1N)Aa)TO,$uhT\N+0b߱GbiGc(PoKMSY[9<^]'e90 30Fҿpg_~ u4/]3?`o-O {%5jvxþ%&?Ei>mI?u1🌥W)噾Yk0q\6;1z5ٯٖ+ceNEmTW*c_N7dN9uj`V1V >k8Pex5,4*b(yw%[eV~,}P@P@P@P@P@P@P |5//ڿwßyM3^#oէZ??hoJ'Ě,OZFŽ7ضpˈ2 Sߌr\X\6sS桀˲X(}k*5)dr<,).E?q /Xlyɱ \OS:sҗQ; V2e,F C'jF|p_Z?>/xEŚ׀_]G@^s5ЬuOJ>2,y"qU6 X zӎ;#pv_q;fy;ìU,/W*T52L Uٷ,L2^9qt2l f*aA:y#bIJurjy~|[v:3K_j~.VGQ<6ᬋ Ɠ#⤺VEsA6iWE|2UxZ= "ɲ?r&ڇ Gy.m'9\?β? 8V|UÉ3^]HYG,f70oqgs,zK?LqOwY//p~{ash8HqN[̲O' E|hf#Jկ5x[m(/4Ѵ]#[Դ⮝>+Z] VxQ^/*s!~OK$BUq^fXf4aq'.y`'quJm$'GsgsȾ=ռYkzx;Mr.SS𾃣|<Eo麍xN4iz7Ma8=C^p*G0~Uj*y]Kק9¤cV1ЧZ:Ҿ$kftxK;iW(OoMj ] ę9ӎ7#+K o8ix9)BiSTQF9<Uq㳼Y.sS=V!sn#u[_|4z|ƓpqÓj?{ 7Q𧆼SV1]_i~$4HKsh^uk/.-$M>%躜9'BzܰeS+rVjL-4ZT:*aVIcK)G1ƞ湖W؎,+.CG,]\>N48 8*cpyj<)~=ѵAOWy⑧jn=|h',|>y.<'3X"hڦia 5yx>|u'vEf9ToV6!I`s#VqWөR3*xxhӖ$_xx-d8SaU`%O813 q/ib2~) =l*f ZyW2乖2Gk€ ( (?7o*_ٳ"|}źD Ïr^)4m{LƷjRjş wNo@#jBC)zlggK 0&&U0\-*VcSS\FPc1G':8Nu]7S ʵ:IFZ 8|EF߲? )~>еz/?g]>RѼUZu7['_IK Ok ߄wZ\׆7We~ yxz,:1X|./&ɱL놳9xK S' S/|7Gyq>g䘾oubTK[j``YX8 ٽLq4`X:3z"dk=3YEͦi {S<70-.?Ҵ(Z|]qxះ>!;eFMgT}[Kr|?xu4<*q|Ë+a<5x GRjdiax?gd3<~?23Oxx|,mL2j09*K >Q Lg<=^+?\?<937&QU˲~)p|63 ">@=oK|)*|?@|Kе/ jY]^x|`߁tx't='Ungľ76J3L p[m#żeq6{ XNqWXŘL~%dxN(rÇᜋ=$x91qy'qdYb e [:*ԞQ[/\3bqٯc1x6c/L~)OiQog{m_v>!/敧|?}-o÷z"QD>B7*|+hVFo^\˩6/GC2υnY&_8pksn ˨cc8 xnY)ٞ}ĸ9kymxNL6 XdT,&:;Ԡ,Kf+1c*bp 'h95_+d 6&,|6+TWB݄>O+RӮu]kşC'晗 ซ%d3aÙ)> g.?#Sx,xG!L |qNp+ k_.R#3=iX F/pɉic1J#'PaקW?._G^|zg-H[_UGk~).ox Fס/&ӿY )ʨ2he^*Մ=ʾEar2V$qbiYVW̲ vGXο猎6XM<<1*S+263 PaQ8, ,u@P@ 2O& o3g~* ?=qkZx[wíkJmQ{Cbt}GL|.B~gֽo xPĞ|3jq_ᇈ X/_q{xBOk?{T26>&67a_OC@7ڿş o<''|gbំzU^%Ğ-񝿆mw<9/ A S}~}ƛWkYٵutͩ>8ٚ,<WYM^V4ڵ8+}=;U)4_/nukIg=B3Omh l'<)F 5V^>",/tdmz_׷~ x85> -ņ r5>aᶍjrgjGN|3w_$}w^n ž*_?ŸuM3/k zɠm<6ዹ+HtZkL歠k:xJ׺?<-S@t4ɵ;ٳAMtN[}_JvoĿk4rC/蟳zj+ռs|f,σ4k umf?ُAJu3? xeuX}Jĺο_ < }PxnM/-8~*~Ѿ:cKF~$Yh)MĞ֢oZ꺝̶`^P@P@|uwd8A |V'muk&#^xs[[j x~+O :OiwX<[# g"✯W0}]dxLoW:Xdc .qzh}C>zqU*8`?6ceV+4c~rfAP.3|=\ W#NF/ 7o/x_\k~(O'oxV֤ś5Am&麗ĿxcK𾓨|Bi/)?1gF+ NY0>%1ѩZPe 2K*.9\;2>%l  w(dxePL_%8᱾RiⰕ|!bY6gfXq-2ΥFkr_Ɨ6:_> |%:xk>S𧆾 xO>07#f`xJyW8\߉px v 5s<;,?Rp..n],_9X ͪ2?-3 fU֖/)U啸7P)an_h~" |P|'>L^ }"<33Ğ|-azy?'[i2/x2}),~zovQx_3w&,5 ) Wl', 43ddƾq5:|3q>, *.kG?/~ ž)]s,?MX9liw0ğ4ψ\:.<5_}6!R{n}7zmG59Mjp :¿&K;Eh$^=亇u V7>*gci ӼˢXxGP[}*Vl}N;?Tu&?ψm<+eO:˭ԥ5/nFПTt6A04|__5߇~ W#ѴwN)u魬th֡W}BVeo$o ڎd|_ !ͫjrǢh7èCqi__|?t[S]}#[u ~5@ Oγ|#s㯍t__CƗ&Fo,??+]]uR$3/{֧|>ҭ7kß>׼o/|?4m: ڭ=/} 1 ־о$iƯqọWZ΋G|3h5#Tw|S\Ki=Mxt VO/YMЬ  BdK?̐mtx]\:?4 i"֡slx;Fi+:ckľuՋ4fLS.Zm Xk2yqԮot/x0-7/Ŀ-0xK:E/oᙴkУ6yMXơjEe᷹ĠowofwI_ zLjoEuW /<1q}jMҚP:PlMV~.u_(>f>𞇭wֳxkBҵ}cQ [SӴ[;NVѿ/QҾݗ|?|8?jZw~xKG3ީo'>2x)OqsxV:.3e WWʸs0Cs<)Ĝ p+ÜqE<} 9W/#[&k%˰U,2,•\KbSYU\~asl?_5ĸF_c\6/'|y09o?P⇂R<|c?>Go?S?_L?Zo~.]!/xWᾕ񙯈\W-ڼ{xoձ,g\csęoʏp?nc.LpW^Y4{)ep85gg<Ϳ1a6 ,4` wd_->y8rjsE߂_&:՗4x_l%siύ>0x[2T% 55m;$|bЇߋ:x~(^|7 <㼻5ax,S Õ|gXŸsC<cV/<+/QŜ &aǸ 218Jur|V_\:\ d˳WG W)Qcp$cfyl?r'=ᆽMi?۟|#g_ϋ\Z<~ᦁQ i{}O3(TLnef/Ʈ/>dTʩbi(zf&om7Ğ6Fڟ AoVM]Zm>!سxc/OG[x] 韠e˳ledN1wPWgTs|ƸG,Þ!ʸe)&KC qø,N _"`2⧔4pqqjJ9NiOfS$aĹv/+kbpuL`,Guj Ak\x¾5|6t??^} ֚o}oEm?ßOAysŞigÈ3 8Ÿ .-e!ghXlv:8mC0S2ԥF05+P3*جf3pX,ҝZ'x!k%>){/|U5+#o}3㽾x>;M?𖱨A?/ÍZgý7CӬ - ,qx ;bp82˼8xse931 0#^+3l=Q#U!HJT*|A-wQ3rII)8EkͶIn~-^k~ς#S ?[||M/> WOMWtM4;w'6|7M2|'Gp\8>,'͸-> x_2p67 {Ř%ʼapKy,y9N^4&SJ.Jm7 Tsr{GNV񭗇 W¤.ig*x.,?|3g< yVy%ϳf9/ ed<(p9T(xӉq8kr?ႌf^IӔW4FNjռ]m+w&ӴkBm:7~εmm?tIn_M^xszGf% ֡mKKMT<"xo⎃3\HBjŗvGk V> j"t]ZS욝/ ( ( (kx| i.W|:Ag_%>#>x3^=H>?Ǻ'mKr;xux5Ykx;6׉9-I")x9<^pul'fxmɖ{x|I *P0pQp:|A,Ì8Zk%plVq#=Ρ['r.&x No/!i>5oOӾ(Qդ? |^[G+#Eo> ?E_̻1CyFƬӅG q #3<Exg$drOr, 9߇sO5 |GʨF2as\p]cWUyL/rUOL~'3ن q^uVN񼺟ŭ? m~:t~"|a<5ᯍ%^VbſwG<_\JPZ⟇ux⎣yb<9q?qS^!3npG,˸'ɲpcsgpf3(˱.(n+3^ec3>aj?ᬫG97Y>?ʰ#p9gsGxykn[S%ɲ *QQSҎo Tri`9 8XxBJ,U1Z&o}O~ӼYVj!ƏFxp9|!V/_~xGƿ&^V?}EH%Yk&(#?p~1eY7fUln}7qOݕr6%SC|'_YvM>~3qh:'P*aJiP?x@.M<|E~+H$1 Oσt_|CKύۭOZ|uw4oc_z~i|%xKI3 O 3lOB_P|#qq\|"_[7:Moj)Fܭ E;JIj_PP@P@P@P@P@P3_ſJ?j/x , M7ys|qE20egig5Yp|&n){8chq7/̨gTos1n?#*e^ d\ G%3gN(*Yi1x? CCU񭅗o-',|!]xNқ?|+< |Ea~:x3=08 ;̸7=831\{1hxq78Kxˇ d :/}v:m] ρuy+D4Oiz||Li@8>&8alҷ|ck_e5e|yLj|!y ey>W,>Q{x 27(wZ7 ጛr&Iףc!,^sBo;fؼn/&>2 wNKW{=D]>=Zώ??oÞ м7GŨ;> A_=m ß |3x ~?QfU>'p;r8\[p߉53dy&s}9O^r2ɳܟo {2^Ȳ~|sX^GC'qoCWxKyB1 ,~;pUcדOq m,f9v;?1k`^ѾywᩴS[gkǯK mU5o|1|:m{'5/Vt|*> gd_`8+9\{5,6oBNxS)S043L1jbx+ɸ׆~eb|dx~&]G@#k7'C7zui>(h>)X|8}|ov_Zig\y<_ 3..8Op\eG6Dⵜf.gxq c8cQV9eY&Yp7 p0B2:n1R*iI*撃w vbNO Cm?_|JƵH#jw.ڄ_>0Z+iix-wWysd4?o&rq/(͸w en ӧxCcyql^#L\|&ykȸT*ЎeӤTh۪)Q/g_⮋.ߊ>7.2WώjxQ&| Qu:R|~dkǿ6K- ,dž3L븮)ㆣ sb3g<f8,E >7:8[gt/V:,|Z~!p ׌p97W[ 8!j>̳jIܓvWG'2g]\;<^Ix..+^aSar7 3(f9:Z>_͉tq8!N/r=e&9m+0yOd9FS 幎G?[xnq&K|q̞o˥&O8g&aL6cK=Ofynaq.mc쿋g,KCD|s:>h7(|xzm|[{5|Rg,x7g~6c<9|?]ww&W5_Οrۃ(gkss."q+}DdPK/+~Wϲqm,3eY/景~Ky eۇ <:S'>:,ߊ~ 5~:ǀq>އy Κ_7֚VgC5qi??ٿľ//Zqs~38υp7O2 \ÜqqnUϸwLxg`/pg縮0⌻|##d!_.qf2E3j"X ˎ0|?Q4%\VI6U(:cעêM3YO MME>jNo|ڍ޵u7)t?lm|iiQ'[Wx3/iʲhSSWfQyNaЫ, /WFOúw ;]=]/]]__> ռyx×V оR\ix5iz7xygpܓ=|Iൗ0ٿ p7c1YI).l8—g596oùeg/_5ӯƮ5ϲ\ EXu:ٵ\gT+&[7k7|E _|iKc/v9x~xO1csO[<>K9QWKqm c9 %0Yg dxcrF379<Uag:]>$j.#/j1;3z͕Os_uhF e+iEQN5 iK $wܾ0/ _#^𖝮x;ⷉeH?h}'_/>&kPR%?KW_=kc~oY>sSq *ٍ ?a8*x2 }2J+&0Gĵx+e̳*G-НY]-u犌](rǒM2N3vts]_|+mh/ xoῂ|!K >̲okxrOh v,9QxZx&FXaaiOӍ(K؉F cq8^&lMz:K]m;omw;jFH9IVP:<@P@P@P@P@?O~-?ᖝHu< >n4<#1x#?OMv ~Y㯅7c?蹘kc[x[3rGb1#㲞0|xhyk|-No93 6ӣ(c2lsL6q:F_,-:Luo2ɠ6o>Xx◎`W 6 xo7NPIk|#?$$? >W#WĿt߄ '/k8🋿#K\&x)pd9e?l782W*0g,(n\ӤS~ 1On˱xm G,G7 OUxk5PViqI .YC_8* M)ifȝ%O1ϳ\„L&& B7/p'b? x>#{ov_ ) wW?zMß_x[x\߂> xYDx~{o3x|3]/?|>d؞sľ ˱|µxڧ.k9o2_ V3)LVKgE\%:Eg9wpqO^OUO2a14r̳ b$6*^䔱ؼ.`sS:\~(ΰ؜5P?S/>~˟ ~i*x7 ŭx@,~!G{h16Ƌoi.~3x Hn|+7gbOd} ^& >xTdF,>+r\[x̎8pihWY p,6'Jx.LnW03* RiO]Q:Cx C .}; u)~;^%]>-_]QM[첌9^GPWe,-Z˰԰x:R"ѧO+USIr1QtQJ)j쒲ݽ;Z:~58'ڷĞؼԮt{Z~ iGKT״-g~Mޱ_xÞ tDg?dx3|ao_pWW斟.>~ƝC2YxVQƚ_tீ uxgX_u7/4}*?Qz-Ox^cT, N區= ( ( ( ( (-_O¿8|i| 5៉Q??h%%<m C+,hڿ_'- #/< GwX+3x^&p|sj6i?<5߈{ $ _>1x?SdcF/>2.oڇ<-׋'<!j߳޹JN񽗈|I <%a +K;E弟]ktoʇ@b,y](dGvS[S#$Cᗆ;=ºggx눳?q뙶ub\|=,&8uj B?eJ9iӄW^09n sG jNOe tJ*U$=Ir&į( ( ( ( ( ( ( /ş-o2C'v? ^W'Nysw&? |i⾯xJҴφzKᆹ\~(Ox~eb܏+嘼WUM8؜)X:/̸:re6>|rO-C77(἗qW^ FGJefob &ożCY_ \?19˲(x7Ï|8Aa>\xgQƅU,NxP0^)w5 I4"?=qx3SԮ?y?'w§xKg\=͡u;xK3*Ted%VSNx<3V4 }FMfى^/ ?Vsn](yV#Zh"*Ra*g|A4qPO\g,^/ N^ּm^ޡxR񶹯X&%麟txgEyoz_5zއ߇?  _AFR i*jiP 5RO[Ob8 SxЭN88lJfx<6N6U2,fYO5rajG82[2^*ETVrml3βJ~qS ( ( ( ( ( ( ( 3o.w~w'w^\x^_xsv7/":^j^XMxmPQ2mW3%ɲi?43#9a9qotC|AxW3qG9VMF(23,C-4q9sfzxZt[0s,V+1̱#,N;UR ( ( ( ( ( ( ( ( ( ( ( ( ( ( ( ( ( ( ( ( ( ( ( ( ( ( ( ( ( ( (?PK!ig^ppt/viewProps.xmlRn1#,"8hTHmK>g}@nٙY&ViS" B_ސ5KU%3O@j UY|%[/I- &mY烬ed.׭4!xY@RNh4% L7fQ]MAjt1 7QR|'X/T.:(s]u`е[Jȸ%Y Ɇ9gOQtd &`G>%{y*>Nz(Cv5x=EQF7k$7Eu1Q@PӲHNORq8<)>D } q4Ju{GU9.In]I}bߡx%qńioxh&PK!ppt/tableStyles.xml I0@Ὁwh}-CQ$ +w*!@he/?JXd45ݤ{c@qqi` yߥ>u L:tgo$mN ppN AA_1"qzs^ XWƉn9nJ]܋u /G&6+h 0&&M"'0{n;jgAksT'xgU[tJkNՙ5s_':TфyDaR4[U0Hi`]We #qtc?rxwPK!+!docProps/app.xml (Tn1}G}d&B9*HDʶ<{6kWIY;KSU}ٹ1~5قʚi62FXfwOɵ50vk ]9ۀ <O*f^TPsGAOi]n۲Tn] `1F5Y8ن *hŮ-lP5͏ alI7M` % d2#EX<6=y YWOFiǛʳB*]k%3%4`Is%%g:],fZ5߉t-J=`胁΁_q<0ق=N-wDY7>8V`l%=SY0PxbnI)E,"EkLmbsRe# /1:#HU.㾓 ",MR{㦑ӕ6~V 7;v|1ej;/6Hw r;@ NAf7ўCzlpϨ=+/PK!l%8MldocProps/core.xml (_O M [FZ]bb7w+B߷v[ݢ>rﹿ{um:et Ahax"繖2rtf& ³5 XEMJ% Bؚpkp׀$&&%0g0m̨5CRʂxwnHYPkbTB$GZ_OuUى!Ì\x+|$1R }M$0ų_!c]z H([P8?QƸQgr|+=x8p`pۃCQ|Dž}.P,7؅2Ƀ_s*%Ae4 g<0FMDxq#[; ]Ǚ8,YW5#bh~'K2BULKZfI#paLYBR,aT]I@ThHUЍr2b#L+nq÷e=`+.h BE4 vC|ۇ1շI3e 0$4íLGYs)T*Cxڞdzy?粩ېQ[ˌSByJ<άK(6]rZZl+dNv8FlYͨZ#ȕYUyEE_0PDQq{BtsNl1ץOEo4zaaf(IdekF QEJ+ Nӛ^jmjԂ뼟Qʫ25W. n KRTʖ`fJM(\8Ax1a*@Ϊfn!YT(ƣɉdR dTT^H*Z'>RL\_I fcl-0.5.0/test/fcl_resources/manyframes/Model_1.xmlmodel.txt000066400000000000000000000001341274356571000241100ustar00rootroot00000000000000svm_type c_svc kernel_type rbf gamma 0.166667 nr_class 1 total_sv 0 rho label -1 nr_sv 0 SV fcl-0.5.0/test/fcl_resources/manyframes/Model_4.xml000066400000000000000000027040531274356571000222710ustar00rootroot00000000000000 fcl-0.5.0/test/fcl_resources/manyframes/Model_4.xmlmodel.txt000066400000000000000000000001341274356571000241130ustar00rootroot00000000000000svm_type c_svc kernel_type rbf gamma 0.166667 nr_class 1 total_sv 0 rho label -1 nr_sv 0 SV fcl-0.5.0/test/fcl_resources/manyframes/Model_5.xml000066400000000000000000027040551274356571000222740ustar00rootroot00000000000000 fcl-0.5.0/test/fcl_resources/newdata/000077500000000000000000000000001274356571000175325ustar00rootroot00000000000000fcl-0.5.0/test/fcl_resources/newdata/Model_1_Scenario_3.xml000066400000000000000000013640701274356571000236140ustar00rootroot00000000000000 fcl-0.5.0/test/fcl_resources/newdata/Model_2_Scenario_3.xml000066400000000000000000061610551274356571000236200ustar00rootroot00000000000000 fcl-0.5.0/test/fcl_resources/newdata/Model_3_Scenario_3.xml000066400000000000000000264726301274356571000236270ustar00rootroot00000000000000 fcl-0.5.0/test/fcl_resources/rob.obj000066400000000000000000000366411274356571000173770ustar00rootroot00000000000000648 216 v -500 -395 375 v 500 -395 375 v 462 -203.6 375 v -500 -395 375 v 462 -203.6 375 v 353.5 -41.5 375 v -500 -395 375 v 353.5 -41.5 375 v 191.4 66.4 375 v -500 -395 375 v 191.4 66.4 375 v -500 66.4 375 v -500 66.4 75 v 191.4 66.4 75 v 353.5 -41.5 75 v -500 66.4 75 v 353.5 -41.5 75 v 462 -203.6 75 v -500 66.4 75 v 462 -203.6 75 v 500 -395 75 v -500 66.4 75 v 500 -395 75 v -500 -395 75 v -500 -395 375 v -500 -395 75 v 500 -395 75 v -500 -395 375 v 500 -395 75 v 500 -395 375 v 500 -395 375 v 500 -395 75 v 462 -203.6 75 v 500 -395 375 v 462 -203.6 75 v 462 -203.6 375 v 462 -203.6 375 v 462 -203.6 75 v 353.5 -41.5 75 v 462 -203.6 375 v 353.5 -41.5 75 v 353.5 -41.5 375 v 353.5 -41.5 375 v 353.5 -41.5 75 v 191.4 66.4 75 v 353.5 -41.5 375 v 191.4 66.4 75 v 191.4 66.4 375 v 191.4 66.4 375 v 191.4 66.4 75 v -500 66.4 75 v 191.4 66.4 375 v -500 66.4 75 v -500 66.4 375 v -500 66.4 375 v -500 66.4 75 v -500 -395 75 v -500 66.4 375 v -500 -395 75 v -500 -395 375 v -500 66.4 375 v 191.4 66.4 375 v -38 296.4 375 v -500 66.4 375 v -38 296.4 375 v -500 296.4 375 v -500 296.4 75 v -38 296.4 75 v 191.4 66.4 75 v -500 296.4 75 v 191.4 66.4 75 v -500 66.4 75 v -500 66.4 375 v -500 66.4 75 v 191.4 66.4 75 v -500 66.4 375 v 191.4 66.4 75 v 191.4 66.4 375 v 191.4 66.4 375 v 191.4 66.4 75 v -38 296.4 75 v 191.4 66.4 375 v -38 296.4 75 v -38 296.4 375 v -38 296.4 375 v -38 296.4 75 v -500 296.4 75 v -38 296.4 375 v -500 296.4 75 v -500 296.4 375 v -500 296.4 375 v -500 296.4 75 v -500 66.4 75 v -500 296.4 375 v -500 66.4 75 v -500 66.4 375 v -500 296.4 375 v -38 296.4 375 v -146.5 458.6 375 v -500 296.4 375 v -146.5 458.6 375 v -308.7 566.4 375 v -500 296.4 375 v -308.7 566.4 375 v -500 605 375 v -500 605 75 v -308.7 566.4 75 v -146.5 458.6 75 v -500 605 75 v -146.5 458.6 75 v -38 296.4 75 v -500 605 75 v -38 296.4 75 v -500 296.4 75 v -500 296.4 375 v -500 296.4 75 v -38 296.4 75 v -500 296.4 375 v -38 296.4 75 v -38 296.4 375 v -38 296.4 375 v -38 296.4 75 v -146.5 458.6 75 v -38 296.4 375 v -146.5 458.6 75 v -146.5 458.6 375 v -146.5 458.6 375 v -146.5 458.6 75 v -308.7 566.4 75 v -146.5 458.6 375 v -308.7 566.4 75 v -308.7 566.4 375 v -308.7 566.4 375 v -308.7 566.4 75 v -500 605 75 v -308.7 566.4 375 v -500 605 75 v -500 605 375 v -500 605 375 v -500 605 75 v -500 296.4 75 v -500 605 375 v -500 296.4 75 v -500 296.4 375 v -500 -595 175 v 500 -595 175 v -500 -395 175 v -500 -395 175 v 500 -595 175 v 500 -395 175 v 500 -595 75 v -500 -595 75 v 500 -395 75 v 500 -395 75 v -500 -595 75 v -500 -395 75 v 500 -595 175 v 500 -595 75 v 500 -395 175 v 500 -395 175 v 500 -595 75 v 500 -395 75 v -500 -595 75 v -500 -595 175 v -500 -395 75 v -500 -395 75 v -500 -595 175 v -500 -395 175 v -500 -395 75 v -500 -395 175 v 500 -395 75 v 500 -395 75 v -500 -395 175 v 500 -395 175 v -500 -595 75 v 500 -595 75 v -500 -595 175 v -500 -595 175 v 500 -595 75 v 500 -595 175 v -500 -605 195 v 500 -605 195 v -500 -395 195 v -500 -395 195 v 500 -605 195 v 500 -395 195 v 500 -605 175 v -500 -605 175 v 500 -395 175 v 500 -395 175 v -500 -605 175 v -500 -395 175 v 500 -605 195 v 500 -605 175 v 500 -395 195 v 500 -395 195 v 500 -605 175 v 500 -395 175 v -500 -605 175 v -500 -605 195 v -500 -395 175 v -500 -395 175 v -500 -605 195 v -500 -395 195 v -500 -395 175 v -500 -395 195 v 500 -395 175 v 500 -395 175 v -500 -395 195 v 500 -395 195 v -500 -605 175 v 500 -605 175 v -500 -605 195 v -500 -605 195 v 500 -605 175 v 500 -605 195 v -430 -495 75 v -431.522 -487.346 75 v -430 -495 -425 v -430 -495 -425 v -431.522 -487.346 75 v -431.522 -487.346 -425 v -431.522 -487.346 75 v -435.858 -480.858 75 v -431.522 -487.346 -425 v -431.522 -487.346 -425 v -435.858 -480.858 75 v -435.858 -480.858 -425 v -435.858 -480.858 75 v -442.346 -476.522 75 v -435.858 -480.858 -425 v -435.858 -480.858 -425 v -442.346 -476.522 75 v -442.346 -476.522 -425 v -442.346 -476.522 75 v -450 -475 75 v -442.346 -476.522 -425 v -442.346 -476.522 -425 v -450 -475 75 v -450 -475 -425 v -450 -475 75 v -457.654 -476.522 75 v -450 -475 -425 v -450 -475 -425 v -457.654 -476.522 75 v -457.654 -476.522 -425 v -457.654 -476.522 75 v -464.142 -480.858 75 v -457.654 -476.522 -425 v -457.654 -476.522 -425 v -464.142 -480.858 75 v -464.142 -480.858 -425 v -464.142 -480.858 75 v -468.478 -487.346 75 v -464.142 -480.858 -425 v -464.142 -480.858 -425 v -468.478 -487.346 75 v -468.478 -487.346 -425 v -468.478 -487.346 75 v -470 -495 75 v -468.478 -487.346 -425 v -468.478 -487.346 -425 v -470 -495 75 v -470 -495 -425 v -470 -495 75 v -468.478 -502.654 75 v -470 -495 -425 v -470 -495 -425 v -468.478 -502.654 75 v -468.478 -502.654 -425 v -468.478 -502.654 75 v -464.142 -509.142 75 v -468.478 -502.654 -425 v -468.478 -502.654 -425 v -464.142 -509.142 75 v -464.142 -509.142 -425 v -464.142 -509.142 75 v -457.654 -513.478 75 v -464.142 -509.142 -425 v -464.142 -509.142 -425 v -457.654 -513.478 75 v -457.654 -513.478 -425 v -457.654 -513.478 75 v -450 -515 75 v -457.654 -513.478 -425 v -457.654 -513.478 -425 v -450 -515 75 v -450 -515 -425 v -450 -515 75 v -442.346 -513.478 75 v -450 -515 -425 v -450 -515 -425 v -442.346 -513.478 75 v -442.346 -513.478 -425 v -442.346 -513.478 75 v -435.858 -509.142 75 v -442.346 -513.478 -425 v -442.346 -513.478 -425 v -435.858 -509.142 75 v -435.858 -509.142 -425 v -435.858 -509.142 75 v -431.522 -502.654 75 v -435.858 -509.142 -425 v -435.858 -509.142 -425 v -431.522 -502.654 75 v -431.522 -502.654 -425 v -431.522 -502.654 75 v -430 -495 75 v -431.522 -502.654 -425 v -431.522 -502.654 -425 v -430 -495 75 v -430 -495 -425 v 470 -495 75 v 468.478 -487.346 75 v 470 -495 -425 v 470 -495 -425 v 468.478 -487.346 75 v 468.478 -487.346 -425 v 468.478 -487.346 75 v 464.142 -480.858 75 v 468.478 -487.346 -425 v 468.478 -487.346 -425 v 464.142 -480.858 75 v 464.142 -480.858 -425 v 464.142 -480.858 75 v 457.654 -476.522 75 v 464.142 -480.858 -425 v 464.142 -480.858 -425 v 457.654 -476.522 75 v 457.654 -476.522 -425 v 457.654 -476.522 75 v 450 -475 75 v 457.654 -476.522 -425 v 457.654 -476.522 -425 v 450 -475 75 v 450 -475 -425 v 450 -475 75 v 442.346 -476.522 75 v 450 -475 -425 v 450 -475 -425 v 442.346 -476.522 75 v 442.346 -476.522 -425 v 442.346 -476.522 75 v 435.858 -480.858 75 v 442.346 -476.522 -425 v 442.346 -476.522 -425 v 435.858 -480.858 75 v 435.858 -480.858 -425 v 435.858 -480.858 75 v 431.522 -487.346 75 v 435.858 -480.858 -425 v 435.858 -480.858 -425 v 431.522 -487.346 75 v 431.522 -487.346 -425 v 431.522 -487.346 75 v 430 -495 75 v 431.522 -487.346 -425 v 431.522 -487.346 -425 v 430 -495 75 v 430 -495 -425 v 430 -495 75 v 431.522 -502.654 75 v 430 -495 -425 v 430 -495 -425 v 431.522 -502.654 75 v 431.522 -502.654 -425 v 431.522 -502.654 75 v 435.858 -509.142 75 v 431.522 -502.654 -425 v 431.522 -502.654 -425 v 435.858 -509.142 75 v 435.858 -509.142 -425 v 435.858 -509.142 75 v 442.346 -513.478 75 v 435.858 -509.142 -425 v 435.858 -509.142 -425 v 442.346 -513.478 75 v 442.346 -513.478 -425 v 442.346 -513.478 75 v 450 -515 75 v 442.346 -513.478 -425 v 442.346 -513.478 -425 v 450 -515 75 v 450 -515 -425 v 450 -515 75 v 457.654 -513.478 75 v 450 -515 -425 v 450 -515 -425 v 457.654 -513.478 75 v 457.654 -513.478 -425 v 457.654 -513.478 75 v 464.142 -509.142 75 v 457.654 -513.478 -425 v 457.654 -513.478 -425 v 464.142 -509.142 75 v 464.142 -509.142 -425 v 464.142 -509.142 75 v 468.478 -502.654 75 v 464.142 -509.142 -425 v 464.142 -509.142 -425 v 468.478 -502.654 75 v 468.478 -502.654 -425 v 468.478 -502.654 75 v 470 -495 75 v 468.478 -502.654 -425 v 468.478 -502.654 -425 v 470 -495 75 v 470 -495 -425 v -380 505 75 v -381.522 512.654 75 v -380 505 -425 v -380 505 -425 v -381.522 512.654 75 v -381.522 512.654 -425 v -381.522 512.654 75 v -385.858 519.142 75 v -381.522 512.654 -425 v -381.522 512.654 -425 v -385.858 519.142 75 v -385.858 519.142 -425 v -385.858 519.142 75 v -392.346 523.478 75 v -385.858 519.142 -425 v -385.858 519.142 -425 v -392.346 523.478 75 v -392.346 523.478 -425 v -392.346 523.478 75 v -400 525 75 v -392.346 523.478 -425 v -392.346 523.478 -425 v -400 525 75 v -400 525 -425 v -400 525 75 v -407.654 523.478 75 v -400 525 -425 v -400 525 -425 v -407.654 523.478 75 v -407.654 523.478 -425 v -407.654 523.478 75 v -414.142 519.142 75 v -407.654 523.478 -425 v -407.654 523.478 -425 v -414.142 519.142 75 v -414.142 519.142 -425 v -414.142 519.142 75 v -418.478 512.654 75 v -414.142 519.142 -425 v -414.142 519.142 -425 v -418.478 512.654 75 v -418.478 512.654 -425 v -418.478 512.654 75 v -420 505 75 v -418.478 512.654 -425 v -418.478 512.654 -425 v -420 505 75 v -420 505 -425 v -420 505 75 v -418.478 497.346 75 v -420 505 -425 v -420 505 -425 v -418.478 497.346 75 v -418.478 497.346 -425 v -418.478 497.346 75 v -414.142 490.858 75 v -418.478 497.346 -425 v -418.478 497.346 -425 v -414.142 490.858 75 v -414.142 490.858 -425 v -414.142 490.858 75 v -407.654 486.522 75 v -414.142 490.858 -425 v -414.142 490.858 -425 v -407.654 486.522 75 v -407.654 486.522 -425 v -407.654 486.522 75 v -400 485 75 v -407.654 486.522 -425 v -407.654 486.522 -425 v -400 485 75 v -400 485 -425 v -400 485 75 v -392.346 486.522 75 v -400 485 -425 v -400 485 -425 v -392.346 486.522 75 v -392.346 486.522 -425 v -392.346 486.522 75 v -385.858 490.858 75 v -392.346 486.522 -425 v -392.346 486.522 -425 v -385.858 490.858 75 v -385.858 490.858 -425 v -385.858 490.858 75 v -381.522 497.346 75 v -385.858 490.858 -425 v -385.858 490.858 -425 v -381.522 497.346 75 v -381.522 497.346 -425 v -381.522 497.346 75 v -380 505 75 v -381.522 497.346 -425 v -381.522 497.346 -425 v -380 505 75 v -380 505 -425 v -500 -395 425 v 500 -395 425 v 462 -203.6 425 v -500 -395 425 v 462 -203.6 425 v 353.5 -41.5 425 v -500 -395 425 v 353.5 -41.5 425 v 191.4 66.4 425 v -500 -395 425 v 191.4 66.4 425 v -500 66.4 425 v -500 66.4 375 v 191.4 66.4 375 v 353.5 -41.5 375 v -500 66.4 375 v 353.5 -41.5 375 v 462 -203.6 375 v -500 66.4 375 v 462 -203.6 375 v 500 -395 375 v -500 66.4 375 v 500 -395 375 v -500 -395 375 v -500 -395 425 v -500 -395 375 v 500 -395 375 v -500 -395 425 v 500 -395 375 v 500 -395 425 v 500 -395 425 v 500 -395 375 v 462 -203.6 375 v 500 -395 425 v 462 -203.6 375 v 462 -203.6 425 v 462 -203.6 425 v 462 -203.6 375 v 353.5 -41.5 375 v 462 -203.6 425 v 353.5 -41.5 375 v 353.5 -41.5 425 v 353.5 -41.5 425 v 353.5 -41.5 375 v 191.4 66.4 375 v 353.5 -41.5 425 v 191.4 66.4 375 v 191.4 66.4 425 v 191.4 66.4 425 v 191.4 66.4 375 v -500 66.4 375 v 191.4 66.4 425 v -500 66.4 375 v -500 66.4 425 v -500 66.4 425 v -500 66.4 375 v -500 -395 375 v -500 66.4 425 v -500 -395 375 v -500 -395 425 v -500 66.4 425 v 191.4 66.4 425 v -38 296.4 425 v -500 66.4 425 v -38 296.4 425 v -500 296.4 425 v -500 296.4 375 v -38 296.4 375 v 191.4 66.4 375 v -500 296.4 375 v 191.4 66.4 375 v -500 66.4 375 v -500 66.4 425 v -500 66.4 375 v 191.4 66.4 375 v -500 66.4 425 v 191.4 66.4 375 v 191.4 66.4 425 v 191.4 66.4 425 v 191.4 66.4 375 v -38 296.4 375 v 191.4 66.4 425 v -38 296.4 375 v -38 296.4 425 v -38 296.4 425 v -38 296.4 375 v -500 296.4 375 v -38 296.4 425 v -500 296.4 375 v -500 296.4 425 v -500 296.4 425 v -500 296.4 375 v -500 66.4 375 v -500 296.4 425 v -500 66.4 375 v -500 66.4 425 v -500 296.4 425 v -38 296.4 425 v -146.5 458.6 425 v -500 296.4 425 v -146.5 458.6 425 v -308.7 566.4 425 v -500 296.4 425 v -308.7 566.4 425 v -500 605 425 v -500 605 375 v -308.7 566.4 375 v -146.5 458.6 375 v -500 605 375 v -146.5 458.6 375 v -38 296.4 375 v -500 605 375 v -38 296.4 375 v -500 296.4 375 v -500 296.4 425 v -500 296.4 375 v -38 296.4 375 v -500 296.4 425 v -38 296.4 375 v -38 296.4 425 v -38 296.4 425 v -38 296.4 375 v -146.5 458.6 375 v -38 296.4 425 v -146.5 458.6 375 v -146.5 458.6 425 v -146.5 458.6 425 v -146.5 458.6 375 v -308.7 566.4 375 v -146.5 458.6 425 v -308.7 566.4 375 v -308.7 566.4 425 v -308.7 566.4 425 v -308.7 566.4 375 v -500 605 375 v -308.7 566.4 425 v -500 605 375 v -500 605 425 v -500 605 425 v -500 605 375 v -500 296.4 375 v -500 605 425 v -500 296.4 375 v -500 296.4 425 f 1 2 3 f 4 5 6 f 7 8 9 f 10 11 12 f 13 14 15 f 16 17 18 f 19 20 21 f 22 23 24 f 25 26 27 f 28 29 30 f 31 32 33 f 34 35 36 f 37 38 39 f 40 41 42 f 43 44 45 f 46 47 48 f 49 50 51 f 52 53 54 f 55 56 57 f 58 59 60 f 61 62 63 f 64 65 66 f 67 68 69 f 70 71 72 f 73 74 75 f 76 77 78 f 79 80 81 f 82 83 84 f 85 86 87 f 88 89 90 f 91 92 93 f 94 95 96 f 97 98 99 f 100 101 102 f 103 104 105 f 106 107 108 f 109 110 111 f 112 113 114 f 115 116 117 f 118 119 120 f 121 122 123 f 124 125 126 f 127 128 129 f 130 131 132 f 133 134 135 f 136 137 138 f 139 140 141 f 142 143 144 f 145 146 147 f 148 149 150 f 151 152 153 f 154 155 156 f 157 158 159 f 160 161 162 f 163 164 165 f 166 167 168 f 169 170 171 f 172 173 174 f 175 176 177 f 178 179 180 f 181 182 183 f 184 185 186 f 187 188 189 f 190 191 192 f 193 194 195 f 196 197 198 f 199 200 201 f 202 203 204 f 205 206 207 f 208 209 210 f 211 212 213 f 214 215 216 f 217 218 219 f 220 221 222 f 223 224 225 f 226 227 228 f 229 230 231 f 232 233 234 f 235 236 237 f 238 239 240 f 241 242 243 f 244 245 246 f 247 248 249 f 250 251 252 f 253 254 255 f 256 257 258 f 259 260 261 f 262 263 264 f 265 266 267 f 268 269 270 f 271 272 273 f 274 275 276 f 277 278 279 f 280 281 282 f 283 284 285 f 286 287 288 f 289 290 291 f 292 293 294 f 295 296 297 f 298 299 300 f 301 302 303 f 304 305 306 f 307 308 309 f 310 311 312 f 313 314 315 f 316 317 318 f 319 320 321 f 322 323 324 f 325 326 327 f 328 329 330 f 331 332 333 f 334 335 336 f 337 338 339 f 340 341 342 f 343 344 345 f 346 347 348 f 349 350 351 f 352 353 354 f 355 356 357 f 358 359 360 f 361 362 363 f 364 365 366 f 367 368 369 f 370 371 372 f 373 374 375 f 376 377 378 f 379 380 381 f 382 383 384 f 385 386 387 f 388 389 390 f 391 392 393 f 394 395 396 f 397 398 399 f 400 401 402 f 403 404 405 f 406 407 408 f 409 410 411 f 412 413 414 f 415 416 417 f 418 419 420 f 421 422 423 f 424 425 426 f 427 428 429 f 430 431 432 f 433 434 435 f 436 437 438 f 439 440 441 f 442 443 444 f 445 446 447 f 448 449 450 f 451 452 453 f 454 455 456 f 457 458 459 f 460 461 462 f 463 464 465 f 466 467 468 f 469 470 471 f 472 473 474 f 475 476 477 f 478 479 480 f 481 482 483 f 484 485 486 f 487 488 489 f 490 491 492 f 493 494 495 f 496 497 498 f 499 500 501 f 502 503 504 f 505 506 507 f 508 509 510 f 511 512 513 f 514 515 516 f 517 518 519 f 520 521 522 f 523 524 525 f 526 527 528 f 529 530 531 f 532 533 534 f 535 536 537 f 538 539 540 f 541 542 543 f 544 545 546 f 547 548 549 f 550 551 552 f 553 554 555 f 556 557 558 f 559 560 561 f 562 563 564 f 565 566 567 f 568 569 570 f 571 572 573 f 574 575 576 f 577 578 579 f 580 581 582 f 583 584 585 f 586 587 588 f 589 590 591 f 592 593 594 f 595 596 597 f 598 599 600 f 601 602 603 f 604 605 606 f 607 608 609 f 610 611 612 f 613 614 615 f 616 617 618 f 619 620 621 f 622 623 624 f 625 626 627 f 628 629 630 f 631 632 633 f 634 635 636 f 637 638 639 f 640 641 642 f 643 644 645 f 646 647 648 fcl-0.5.0/test/fcl_resources/scenario-1-2-3/000077500000000000000000000000001274356571000203475ustar00rootroot00000000000000fcl-0.5.0/test/fcl_resources/scenario-1-2-3/Contact model library.pptx000066400000000000000000034016431274356571000254000ustar00rootroot00000000000000PK!m5@ [Content_Types].xml (Xn0 }0:J8}i>@D, 5?# Z%<"cJlXdFf$ sUpȯǯXdXrxj`-mF[JmTidL.͊je+W5͕t U6b^@rόJCv i-S9sneqwKCMRm{)( Cp}9Nm\f! vPT3{*&0)N_=R4TgaOF[iuV;kc\}<U=PQGKϊEOW!pL[bF8k\q/r"61NjT͖ÿ(39ģ]+͕9jS?PK!ht _rels/.rels (J1!}7*"loDc2HҾaa-?_zwxm U ^s1xRp bD#rʃY NʬrɗJC.aeD=U]Sik@X6G[:b4(uH%-+0A?t>vT<@<܋G;Π|ժHo@˿9'홼!s4HN.PK!4OE ppt/slides/_rels/slide4.xml.relsOK0!L *^da`hiC]7⥅E/0xI\8el@(x;AB^<+9þٽD,*gc) 1#;2Du3T F0MsiɀnG iwvsώ}bU d(kK^['p.N 3 OPK!  ppt/slides/_rels/slide3.xml.relsj0{vȹ@>"eQI W87_~7Š:D 1 LJ;TS\*QBQ0՚^S1qh1fOl1$˸=5 S|6;%qרCηl*=G~/SkY-Z7eV~˶O3ܬ>|PK!K=7 ppt/slides/_rels/slide2.xml.rels 0DnR=HS/"D?`ImMB6{s xN}xxQb@7:iN-rQ_i\BܻȢPV_sPK!c\#7 ppt/slides/_rels/slide1.xml.relsj0D{$;Re_B Sq>`ֶ- 꿯6zN.xQbZV ț`5 gIJ ]{h~h\B<ȢPpN*Y-N7PK!BE ppt/slides/_rels/slide8.xml.relsj0{$'ȹB>beR귯J/6Rz]apx. ـEv~^Ap`p4LplWwW#\bQ)5 G#˘(MRe*awAKj4; hLq2܃8O:Ύ}:zS(7,ջ1[*TLco1?e ,:YNg~o?PK!B{ ppt/slides/_rels/slide9.xml.relsJ0nVYD6݋ d}!a?$Yoo@x}|3ݧlyFX.5N2ۛMXRm̬R|0j$HNZ&#"#]lDZ2_1^KH{}0Ǫ7; Uɑ/g$uU1*8~ >\^GWéRYX^[BtvROBbPK! jG!ppt/slides/_rels/slide10.xml.relsOK0!ݤݕEdӽI044lLV7⥅E/0x?|ISfV6 (Ѹ`5@p`p4pno4aG<ĢRkKIJq?G1Q!flUԦiv*/Эh4ق8ͩ:ΎzzS(W,ջ1[*Ty{/S_O9ڿ3sU+9ί'PK!hG\ppt/_rels/presentation.xml.rels (j0Ar?cQۛ1`ldzje[~kK9 9ɏXUV)q"PKUc0c:rYi);nt&ܕ#EavΙG1A-1?նm̾d|$n=gSf׹s0~{Ig5(wc Uo(m.ei쥌FaHžW %mREQ (4dbB! Xі(EP"d E{~7;ӷrdM@'ʘ&|8"㞲QTgP]N;ER^9RyOAp~l͐7v?PK!." ppt/presentation.xmln0'"N4$䋈Pi*uQ' >Scvu ?`މf򐇺WogU'_%U˕D:deKVRx{UA JlDN \1w]=U8qCmmr2s@PNx ''9;r::|(GLzV+Q4b1 Igf.v,ΖoHkS]}=2ڨ nZx>y}w ;= (1⡛-}ݼ_#b-@w}%Z0K7 )B6N60ejU3fzNމb4u2Q=mGKdtq4z'R C;&S@|f7>}[|NLC|`@z4 (veafT,( 3lT,F6\i:1'X@sP!m:HS1'&[/PK!L0Lppt/slides/slide2.xmlWn0 } ΍u{%hBX,&?ʗ4nRKn/2ERdKB~t*&v|~5J mp2WV&@Y?Ȝ+0<ÜO@ERmrLİ0:::s&TP7Tp<|r9f5 ܔ[) m16I~bT Mj64 +r%YȌ'''Sz,&l0D_iqWqglycu A= :ng,D*SFS/4mAiWլq1{EnQ3λ%%NKN=AKFn!$f19 /W(Q0lN%2rMF^'SqTm{ďNQ%Cfͳ=VSɖ+:'sCV$C8fZ&h"!4Uv=T8H)g v}9"!J5* &SԴ4\qPUK@'nդb..3YJ ~m~sR[9! fq pq )l+Vx"K5R@5vGSyx}uSw)'Zp?u@R6fgRL2'-D\Zj=_k*6o*.=CS2 ſUbM.9-zjC \b"[簢S'hIεVzl>\KV\ΠCsZt{@2}4>PK!` y&ppt/slides/slide3.xmlWr6w]$Id&= IlA Y\-Yv]ǻza}s.7Zj eW' sK, 9S3udVng{3\5w g mkhXe/ǽW*iSbQ.ֵP>Brݪ2fbX`&ht̊+Yү3VZ'k̥ 6U%0k%5XxTâwl-vaS>Enl;KPgE,nwbtu:fj"tY e9د`Y\]+3 햵f%xh;֡@Zwmee>VRRhTsReXNᛑ|4& p , I#ŊL/(*>utm|%< Y:NNN7J#z.[#:II"o&V|+M ̒|2Yճ!/Oʠy%K0JنKp&QKymm0JjZ|{oQIɬؓPKj,f/~N-ڳͻIFB|-+Y"ԫWqϒ@7j.Xl* f|@N4>bz`lT3vUmY0YA{j=b zdvP%a;[`AG1o-K{`fd S8LLrE.`yJ䣑yC>ytg'0z*75ܶNuBmFy?\nt2ǾfO$S_l?i irs 10A1e0vGlSPK!Hwppt/slides/slide4.xmlYn8uږQhMQm}Fm%P}!qݦ=iqf8pM#5W}y?€˲j?E `AdL4ߏ`Y<1!XPTD Q so |p|N_7[YH{HѴG+/w$ij'V5Ƶb(%.8bW8Njwڂ&fjZiS9}gK\ /t&TnķrO=Rl:5 _X'IT_MT+]ou?ʶuHZ^œAE;hRWwo/D]{/1wۑ[{c_"`+B}`81: 醄d}7PK!O7ppt/slides/slide5.xmlW[o6~@i{h-ɲe q.mm4%FIw)ɉ$ne~e\sٻ]-ȆiÕ0 Lr5~^X*K*d`L_Κ܈49M>b55oU$-z5*5݂Z0jeSr AIh&MIkNhf@g9xV܈Ms'ZokMx xD` FEKJ DRg4n{&%?,Eu5@[TGW /K׼8v( Z32k( ꢢrޛicCuJ.o.>zjR@.Z\ZwM@{%@6Q% \`$E[hn @]lеUEj:B(Sx4 pq6Oч0~b&aN|1  I pi"x=A~e-jG"3̷ 6ym M, C(@lt43LO4 4fH B x/.j ArYq)=$%opx!{&`ye0M@dJ+qbj{!V*P]AZ\M6 ㋹| {Zؽkk 7:d"R?ay>@)\KZ0rOc2 E ~+_'"V}Q'o۰~#Vs`(I,0f+dwŨ6vwCvpC\$Ԑ-#+E A(:hQywa~?N!:tLS>hJl Д|'Qgxui6vԍ{lIX^=W?qF'ƶ҈":}SX88doGqw[n#aNnMgfP_~-KkffQVKcn}Ïî,];]i2Uu8ÒmD8 77l1bsaxq9M/'ab9yA] 7T,+m6xF8j ʀPK!E/v(ppt/slides/slide10.xmlZn8}_`A{kM6) 6A~#ѶZ(u~gxd[nԛp^l /szzmYy]|~~x"uVR^ЪĽc?^5Ӷ*픞 )d ˺ajn|R. x%w|=9oKDJXy(jk5JzcI*m> މ溹եpu8]± SdK|n5z&O\8;!:ekǼ5_\ `ݤʹ)s2PCy+DS6E/>vj)οٛaѻT3~漬*6^;bʖ7 X/@ ܈K}:ȯ`IМY+@%i -'`ϩ3yisIpa8K`i`'jF^N`-.NEWfUvˆ^ c9jܡQ46Sؙ))f c&5~LQP`(4۶R'>HY)2_cpVjKyU} V8Fp9.h /.~%$"p 2^u@k7|cg+E 2Z|vٲ_ʉS ìJ5z3Ni= ]TJb3`O6S1U'.s fk'5Ŵ?ALXc쫛ܜPּ^c /v3=^\︙ۺCu7 .,duVV`(5pk.)O+QP_AVs ƋK*<R'./>]TѶ@Lv΃ 5ޤm[P}ug*v43^C#@mv :z 厘v Hu(AjY@195΂oGlTl2t1.WA!x@ys+eM"ye>,)J!a+@RU甧7ajJum>mTc5H}:#i@hUDVżkWuuv ITaŏ,8; oPKNSYp˺k#dQXc|ͯMrۍj[mk)h9_HB-_FXep;PBCb6q $YL ]?JFk9*HuUXX=k̷(*݌3efu1^#{ꚦ?@_ *r?TH̆n36j_JKT%؁ &d%`KOo7Tdun'gt-oRPk&gr~V=`HMw Z0k& um=5GU45~|d$('i{Qx3P2hW ?nz}<<9`$$<al8~6 $[%nSZ0 *wdvC55V[9f , *:"ONiYߢa2SĞvpv}lglk#-'tYroQoύߡQoa#μW_\TI&?5r 퇠x?PK!YaM,ppt/slides/slide6.xmlZ[s8~ߙ ﭹN6δM&iM J=Ggf~I0I~>uWi2 "Irn~v*4J`tn|{oY%PjFJr6Tz]Es"'_NNkM'9IYEӿ&L(&fDʫUZV 1JN+`#{K:WYG^^\~z{4y#9Ŝa'a0 _6l2 ¿ǿ@Dft-X۷|dl02zL+ГԎ\7,qIc2wd5*)f^1;R۳=g =Yw:Qt-W#-r&L\ |X&BH4yyK2fX K$dYe8% Hh2vIpІ#fNO3nIlɮ'l5=Fiz, 1BZ8OY$?.xsr&]v. :",^ಱJY%P $ $[BDу(K.'%~U @ͤRhe+SHMs@+|7Sd `%iV+NW0/}W ]+΋%I <{[?+MF >5?%X2DM)JNb ݷ9UShgSi\})`OnD!J ^!W*e[ '[q=gSK nnFɥi"< &] K iAx$ԉ¦CsIxns=J,)^%F7~K ^s \/"pmO(T <}6i40By)v޷:eDԭ3#w]uָ1 U"YZ:e0p|7򆑤\ȲȨrqQɦ&Z(W2ْZ^]dJ6Bӕ:( ;CԪ'ڐJSu<`naQrV[7mMEw+g4^i Axo Ew]Aӻѫㄺ{;Y`V۳=~-2#b_J fSGDk۶yB~f5qS|l$]qZ0U\ [b7R՚%Kp ,cf =m[p<9;k\Ӄ*rI:my#ȼ:m\s{aVKvæګu&j˟GF: ѱ=2]uݞhh:B~;#~6>SsOp!ՆC ?8_@z+@++}ApWgSmkF`1-{zݛjE{^]0E!uo:ՠ.}X7HM^P\:oB[ym.­-hU Wx!^vPK!|Ƣ8ppt/slides/slide8.xml[[s8~ߙC| )]0N ?M˿H/qzKC@XH:w}Gyfj+$Ϧ5Eyd+_*036R{_oIrNEUѨlex72G1u,a5=}>%',$ҰER[ng%ޒ"oY|Sȋ⌋_Vg\KbKײp E=L|0 5Nnf|y&`oT '"e~-N76 fbR$Yohl:c'8Ӭfs5AH,ͣNj0weJ D}y~`a\R$&J2M$MI [^2-]`]$Jcnfw,ˇ9^ԯ+ΪhA g`LCi-T/ vZR:{y91OxU:Ѯi-7láǞ<4zO t-GȠTF&f WZ@\$i]O\AyL*Ƶ4YNu, H XWag ̈́yҖJQ@y~,H0i."2U%jPָ( K-]ygG 446\(J8k 6[^0ƾ5 ֊eO ~$s6CHZb>q50Etz4I MiCT%26a_͌& 5Ze|ZL"w\Ly]/\WqBGa-rxaTq)6\VD(>,P9IJ,9ޤ0βW.jrW m cCKN,v LnS5D أ쓴LOOUt yh^*5Z?A* RHIMc#d[Om;vHǁm~GPiŁy,C  e9$S &cZxW>eu1;'*[BM˫cNTGJ}Ss-?pZV=iDJ- :i[.SM?X%gE\^@Kd{;lhD No'lt|=a@cBt^ =?uk Ⰳ^*pa@4/Q.ͯ~84 ^;ЫBO%/lmz[W76[hDPh[(~5Qke㬮n*|D]DtD=qOnw[Z:0=w&zWzwUEDtӳLZe MFn҄/Ӷճ+oǪ)WvnhC*v쨹v:4ןCs]A.oԿR9,NW! gKh?v/PK!kppt/slides/slide9.xmlYr6}Lç!ױI&{b`ؒ e_@P*;{& gl`Rx/ eW|6 |Hi+tMSj**$k-HsR ݴ-SFW`mF$ QKk]wi]߻e\;#5Tռ[w&$S`ƎXxV5Ĺd̴gT_+@`ԿٟAc5|-b9>-7 ha~ZOv-= IE!ҹtZ!x/%CT =gXju pN}P^BD#2^ӂw`cC! jeA͕@]e&@R7]c`:(R>y(}W @¢,Z8  RrV6B]'ha:~і Nyg߁5y2s7Xy!XH C8e/IlGV`I@L]n~͢#p`;~-7n+9dʺCѯw }n=xje~7~/y'*ȫ = ymv8}M@ltG4@{_#q+ \z@Wd$!ΈkSm@' #Q&c[u^g <~2Ϫ-8|2/3{iI&xQOMYRBkkqDb8o|}iwq 'I o|}QVΟZ8KVq OQ+25)UW[ǝ0nOd8_x64Ù>fU}پ Ñe>IBA#$qI_.C?׳s!lE 4GT, |;LG blǙPK!|ppt/slides/slide1.xmlSN0}?X~,<*,RK =M4Mc'/x.scLwXINyݹS#w知HN9b7OP'WTC#[PWUR-ZH>G xt_jMuZ]W:'z(mNE$E/x=fϼ'&ўFEj_@RtO{)t$AqS+k4tf}gFDw3W~i,g٦Yasew}iaW0+1x~PK!ppt/slides/slide7.xmlWr6w=$I92g2Ǟy$ _Iٲ^$.vﶵ kf,WroaTq("QYQ$G;fwԅii :׳ԾUIx7W͢W%~֫)Q#oNW9/ٕ*W5.(1LP%׶զOѦ K?1i cF;_Y++"Kk^4c/ZMM=F/тm)f-GΖGN R]h^xy0N'xVs*D{o5+]2FmV% ^ns&B &`SExkå 8YS~ (Ka:\D=sЇ'Z c 1mnTʩeǃa~'$섈 l>uu .-0Dןmcb{ I6(&C/M3Dz*\5"g%NdՁGx>lIMmI<@!U8q[m|3n{ ~#6-q0@D<#4B1'ppL0L*(J+nK%+Į̜<%3 J"BaՂb~= LBN0)xS̞L'4M4}V4!5,OsUSvbq+߱dy)ųfg`]*$.c\(0P;(q$Xn+& ѴУগ4 ,u"8ͷY{=|M.Y:B s$9f'9ER#I, L4ęMNԘpav *z#<NЀ]Ij"1o1z`qtG/w/Jw>>W) շko|Hli6A7PK!ђ7,ppt/slideLayouts/_rels/slideLayout1.xml.rels 0Dnz^DEd$dߛc0ovkœ5ԲA|v= 88OİoB#EYÐs)f YH8]H"S";UQi΀)NVC:Kv:gc"T(3rTzy.jY6knPK!ђ7-ppt/slideLayouts/_rels/slideLayout11.xml.rels 0Dnz^DEd$dߛc0ovkœ5ԲA|v= 88OİoB#EYÐs)f YH8]H"S";UQi΀)NVC:Kv:gc"T(3rTzy.jY6knPK!ђ7,ppt/slideLayouts/_rels/slideLayout9.xml.rels 0Dnz^DEd$dߛc0ovkœ5ԲA|v= 88OİoB#EYÐs)f YH8]H"S";UQi΀)NVC:Kv:gc"T(3rTzy.jY6knPK!ђ7,ppt/slideLayouts/_rels/slideLayout8.xml.rels 0Dnz^DEd$dߛc0ovkœ5ԲA|v= 88OİoB#EYÐs)f YH8]H"S";UQi΀)NVC:Kv:gc"T(3rTzy.jY6knPK!i_!,ppt/slideMasters/_rels/slideMaster1.xml.relsj Ab5®FO>Xv,o?) (B+xzvJ2ȒJN6 >/o/; q)x$2©|~:~`ϝdN[HˠuN(U(Jj^}i5Pj``wWU]N jN;4$tNޗbV!6le46Co,XClɀ3+bʊ`fqC &=Yc!>l'o PK!ђ7,ppt/slideLayouts/_rels/slideLayout3.xml.rels 0Dnz^DEd$dߛc0ovkœ5ԲA|v= 88OİoB#EYÐs)f YH8]H"S";UQi΀)NVC:Kv:gc"T(3rTzy.jY6knPK!ђ7,ppt/slideLayouts/_rels/slideLayout4.xml.rels 0Dnz^DEd$dߛc0ovkœ5ԲA|v= 88OİoB#EYÐs)f YH8]H"S";UQi΀)NVC:Kv:gc"T(3rTzy.jY6knPK!ђ7,ppt/slideLayouts/_rels/slideLayout5.xml.rels 0Dnz^DEd$dߛc0ovkœ5ԲA|v= 88OİoB#EYÐs)f YH8]H"S";UQi΀)NVC:Kv:gc"T(3rTzy.jY6knPK!ђ7,ppt/slideLayouts/_rels/slideLayout6.xml.rels 0Dnz^DEd$dߛc0ovkœ5ԲA|v= 88OİoB#EYÐs)f YH8]H"S";UQi΀)NVC:Kv:gc"T(3rTzy.jY6knPK! 1/!ppt/slideMasters/slideMaster1.xmlZ]nF~/;cH%Xl9j8;X+dɕ*(;E۷%'"mɑa ܝ7\Kq LU4xQL構Q+0B$b"M~!w<7GRI"9+^O`ms&1mw:~{]I4\y" &,Ί[ ,ݍ#sx>?G+RcOCK&xjÃ6nb=EvsdSgg9>g99Ђ&iɉW|'pB@ ?a4B5gه vZRu.b)q&Xg@WHDtC ea$)E )U̐WHI"[MdIE_|CWRq.(4vueN,kFY^ȟx:7p00sJR<-"-I}u/Wit`04lmR ̞nItR+ƊTv$>39%k;ZtWE!X$a H 5H 6m; iۖT=e7j(B,0q.(N Q,=`NJeG2NJ:V9m&Z_Qc]dQ87Ѩg\ v,.f ZHִPJ p+-ȧp+-Hj+-ȴoHy7vE-PRϡ!&n КZ¸\cB/W!(P G8傍!1-US8\9G A:g$,<ƆB6ҕ -\ IXͫA^;fYX:(%W 翴D c8S aqm!,pa[6۔*t pCsLǵ{x8jerQn5 F)$S+112<>s ex1_WQE1نcڂcҺG2+%UBWaeT=u!VvP6G 5VekP5 _,1Y"Et>񼪖r?d>K<&#/ R%/:D\WP-#g+ 8@& rI WcLьՌ&ϼVg,'B۠X&L-! ^ [мz$@(hh,LN5Yruɒ*~ SDiSNJn /dH !OBH2D\k,$H' {l IL0_^ p+C&Cx)|H)48& <* 1&n.A430lPVIBtI~g?u;N?j50Kꢎ 5%KB9~ptf`"2;8ֱy|;evK4!`;. LS#}mdmR8& R;݋NL?  DRT+#n_{C4c;Erl=!GX958;XeD)kϒ ؉U6vMs%NԼ.GvzpFFxaG/$z H[mT]7 5+4R-Ԟ3m 9bưziSg2cFMf?V|R1A*;._iF aigbl韃(y6DAdCYnN$^1i 5%c1Х)Ռ% &&~?N24- pHEŝΜ. Y 6P4[ )yɹ~f\-P(vXVf'fHpiòÇz`P(AϐË ;Aw:./=C^`D~#p2"CQv#I\h!pFt<:GÈRj{#3\p__7~hoy4gs MobS{IbU{ HOt,'辫ZBA̯t>a| (ntIb22'L P fъC A$ٯSR_,d ^]kuS se점3H&@3Aeaݮ&eƩ[N 3 ٙp|} zWV(.h퉍h^2xY]-YSTUbǏ1uU kEJf+?>x '] TSiSM}?a*+ YG$RL2dQМ d IcIΈj"+5 1I9M if f B]NdX,_1\hK)㈦NIdE#gi&Gt@R[g$gv+J؆ /w|Lmhs9?`ڹx] {u]]nGFO2JUYNlnYܴt&qU-dAۜvݜsmxԩ+Q Xn 鼌5#S%.^ڧq Rc\mk6.m/Aп;]{ð㎶ꕺr+_~Af,S^D .Vfˁs&)4C, QTPK!Jj!ppt/slideLayouts/slideLayout3.xmlXrF}OUaJyfAHBeزIk~ FKFMj+==vZb\~1ӧOϴ8y%[ Uge1rw=")Ӭ9naEeY~O'U\o˕fQ19KۭyDErSuS\v^/<+v:fXd8/U. QBr 2jk:ZD 3f}Hz[!Z$ :,Tk '33Z([%+ֿjV(j}Xvm'egex>~g-xP`mFOlh4~4Y^?6Y^׬('߄\1WKPT4|1z~ {H c5xp`XKpҘb9+-Q:72NjdYBvXzyZ@ĸCIǩX|Pe%|Mv'}"1(HN(Χ1)8DfgK&LB1C!ɺ6>IQ7\qwhcx 6zCeR%He)Q ̣ Q-6ORli+' 0ɳUr_^ύ63M#yc1A\]͊' =RN+ɁLp$6u)tIIki*&R5P߆ 3+t3Q[lRy`s'3SGm!P#+%6{Ce~,nCPG/Ef j -ȋ(o/l{޷@xa?^S5o0f$>"̝Oq.w{L Pr=εwϛK|ڠYZr}sQ#l袇aiLwa;ۧ[uz?}ߟ^:ӡn' 4 `8mhPuivRzRُƵG])H6aiYRw؆?ynzZ5}<'3L2XFfhZ5ƴh _R`c8uIg8 :t{ytW߾˷fMܼޛ9WCdh~ ٰ6PK!R;!ppt/slideLayouts/slideLayout4.xmlXr6LA^_'C7I"+K$l3ZH!@en#㪢hI,9:yQeubIY)gd謉t>~N$->5_(L&x̕nWsRaׄVS݃{]`(n,zig͙nj~W;yҒi5}=G욪5//AS#Z]b f'TZSbmOj:_&ՕJ)xj2G(")cԔ純wXv}8avH&tGqN mo @WrDY!_MQ6øt"827[MzMM5i̴z^, nۃ X/=X`CX ltkZ`CX htҎ130l٥#$|]M.iQ$ze'O8$ _5?zAG#+g{9k] M+e葪R<$>RM)|I s1F z>VvIS+dXOB,ϱOrI.؁hmi9PeXfIUv .ulPEJȈVXPiAY_reTAC!}x#C\P?nSf-7P) WX;HvFL "UjVh6r3UZ5ZWwɆSi+Z]wwX\%SuLTku+Zld{b\%mFk[λ HK5A7[KtC!73 X@nJև9g&}ioL>YF+#2+hD7m%P ӔcB^piCWоi]C롫Vٛ1Xwl~/߾{1u)E}b" 2vHRxqAhA*mPK!Ϙ!ppt/slideLayouts/slideLayout6.xmlUn0}4@ P@Q; M[Kv$:}=ۣCОJ >Y#|3Ʒ73HiSc| _L޿b9FUHxuJ LZ&dA4|ʍJ v;-Hq/ߒ/,OL$r]Hʈ6/UVT$},5KΎPIO}f)⤀lYQ卤Ԍ,Jڄ~%Q:Bf?9}iHxd1@1/$4J4l&+nTaU1NS=B ^^!._Kp6=MMXp)ң!~vL>2jI3b|Ms_:bk$byr@45&JS МtE$#! E7°B3)Z1Э`)TmCTp,0\YVZӀ8`r:8x9`9~?vfh:0)PyA|tm `1pp x5Pi99ўL˪?wDM1hW~Ț)E]qB^3ЯJcE̓v? ^9(tChNU9մ?><>l^Wɾ)L^r6RA_K]^ -2?˧~T/Np0p)3Y_>_,P\*AJpW˸4Zq ZQ 0juHr["6Sf7LcyqrU %K6CTC)Ygb^ܗjdqDP oh f?[xYی $nKX,om9bMGwڝX&;jSsŲ~{Z_,Y~IP]TLAw0p胷8ڸ(6z6Hؤ.ʣ-e> $mQ&qINI~f2g"%+)J]=Ȣ{^r:!2kU(/'IepP,$!"NY@(pY<":i:Z;nV8g}6!eSY\0 E-?-/x z$m6}+ܠ%d XИ2+t'S> G5LLE`abHFWlB6؀% nx< wGm,nGߺ5Lc)8[*(,T4ʽaeȔϊ>_*RQոO/uLH8n &vԶ ^F4=\dFEto$"CΪ8RNF l V 3:I psh$43o1<0 RT#qFLd 8}"?Œ|`qC2.+2ɬR5>s w`!_d"U@~M|*eߥQe]*nWuh"a[:UU .u6ob Xib XVU=5`SXw kl!$`6+{Jinr̓. x,+.Π׽ut놸}- LswTa^BϏc̽9Fͦ*M)7ELMH8F3N'R5%9b+^FۥUFxs>x8^14iG]*f u6FtJDE'*y̌>oOO4SGJݣ޹h󻾒 ;㚯aK4=.@k>EvvB=~繽14skIDZ;_DFhuZPOG=yH>ppm{|s}i]xwu׾V}֏ઌ>\J0.DIێ $gemM\U~X;ԣ8 _HDd"∓e2;qP ͉1zIhydײK߷}vW+NWP WEQfl*uńi@`*kݢկAojd{AS!ls"eؠocv%6U^m抔Pb?(_Ho Ud;blkHh~!7[^ M-џt-)F2>|3F'qiQwr jÁPUb1O'PK!J]ڱ "ppt/slideLayouts/slideLayout11.xmlVn8;Y%Y,.%KkwDGB)RKҮݢ@_}>INj7ދ,3gofȳw5lo"e&E& S鼛~u֦ &Sa.,0$r"f$<TUݐ[ r҇! ġU hӊYqr}D2贌Ė9KV/|)/pԘ)t^Y FnP~,>wq,K:s[xԬ9+%K9pz̧.P2Dc 7PK! 9\ppt/theme/theme1.xmlYOo6w toc'vuرMniXS@I}úa0l+t&[HJH۰ŇD"|#uƣC"$I۫_y$>h{ I37#һ{HLO&n{Rʊa˫<% ̍Wݘj+1ٻ1 jVN5QRL 4i0`R9]&!fmhH)1,Ly+[Wf%kK旭USih])S^ z}4i6NNfKdnu:f+5 Xo۫ހ,otuo@_k7\E&vhQ/ cv+ߨe9 .b,b>4aEf)c MK3vȗ Cj{2bN߼|޼|v㟎<9~,IX^co^?/_䗟?B%z^<{է- ҘHtn0d$ηbaZ^'XsS3 W:ĵ} xsx\hv+Eni^%3IX\L˸xwq7Mt*݈8b3(( {=Ա|:VdHGN42f>pV9tU?$1Mܣ#<@Th_BvpLˊ|押-heJ잨p'o&}qe|֊;/Pucd.K1elfܖaa}I4Ǭ;P` >*D8fi"H\! Wxhؕ="6 3ZlZF~fu-ԙՍh9 `aMD/`u8kpHvbp.H#Iy[ )V+qki,N*k,a{]GK:oO#KtZզ|1o1NR7pI+ad6Y>f+WM:\YX/(ԁTHedCLe!ʿ^6B L Z2_]ѶY)SE ЈMP}*Twjf-Yҕo ΎcF8+:ELpDž $V)Q Ua?SE'pehp+0P*F hLh{Ye_C朥aN~ꀆHP؏T$هdbl$YFDTI\ZG䐰zoPnIV dYBͩ!ks|l2Rn6 MnBĊ]ծ7󽷬Y<+Yi+hei"skAf.xqQc,.~>_):P[|t l iG8AL5m:iw֒4vќ\Hcgvlmǖ<{2Eahdc(>zށ)S|_ZρH~, PK !zdocProps/thumbnail.jpegJFIFxxCC" }!1AQa"q2#BR$3br %&'()*456789:CDEFGHIJSTUVWXYZcdefghijstuvwxyz w!1AQaq"2B #3Rbr $4%&'()*56789:CDEFGHIJSTUVWXYZcdefghijstuvwxyz ?((((((((((((((((((((((((((((((((((((((((((((((((((((((((((((((((((((`?ikoٗ7_"c>jWê\xw@+t )Qws(B1VcqL=*pRJ؜Uz8z4:jBNRHTwcJ|Mj)ӣѩ+U(ӥCVz%tӝJ#&_SWR~|thV>/_ Cg)}{ş 2F+⣭4CKDpxAG_ڛrž '⟈>73|5]vο Sдkދeڍ]_$$h7J*T*B~ʕy{:S){%:.~WSҨT7eԢRTQcR)ƴJRJ5e R橹r+YI}E~~|gg/? >#>+o5&$&^ KRP> /\n=?Za 'ğ߲>h:| Ljh/>)m+p|:6+}{Uҵؼ1\߈.f7P彟/SIr{׫8JKõ_yVZjPjMܒӄ1Ud~9C;=g DeNjrsQҊ_;x6> y{=o c|k O^XIh}5ZN~%~?b |jyqB|%ڗ? j͗Oo}w yuoAsh5mk,WҽJ1O<)}N0/FV5)oઔN^xNJ?k6N%%9qUjPK^:5*pQ_>xxN//o{kmcZ_tx?ÏxT񶷧ڶ;"&xWҼGOK׶rj2|0gs>k/gᧉSo@)׾ VDlΑ84@]i.%FqRRZc_WM]%Ox_hJY[Sr9yof:i(b#Nt+(ҟ/_>?e_#Zggg}/2xA;kiqwwaKW5$P Mo2'G[gůd1|fh/o|'ߊo%>\KV }}K~P^xF&jvhWd<1ݶA(Sx5]LCPtXB^1+*ŮYQ*Z8W_=c]ҌRGN/,[_4W8~z'_Cxd7Ǻⷂ-XxOgu i0Xl_XjåVu67[rߴ/o#7ÿ4O^F#2:.> Kό8UEc]7][k  '%NQU.U%E?UJi(qXeU)7Juƚ4baSݞ*y:ppZ;ҜwҔhyS -፦1|B%c[ω#YSk}g o|69o >9mcI·e?^ dω~?|>&?>~~<_h>&<5MkXtY5Nku58ƕ*j.kQVJmJKV5T'CSաNtk)iIFM5FSm•I%hIh?ۓ)~u[~*.S~X|%IXP䗶ty_Wxj7G~κtj+`;7|!-FG>%x2_|>ltNK񿉣֛Eg.uc^\]$z겺VVV dAH UJ2a4*T5}R8Ii8IJ-ŦgZQx?z*GF?rkxKݕE ( ( ( ( ( |G~1㯋_~'MgZg2r/|CWᮏj] 8ּ37o>NЬ<]w×ڏt:*Z|z083.C(UjBtsVRQthbi8S}x7(s,#/JJrV0NqJmT(1k?j_ƿ3>([Gwuſ7cO7g߉C#;g>2^/6^.4'ߵJןcg~?f> fݫSk|n_|>|h:Oi(4Rռm[|It#[E8FJV>xvPv#ޓKVU0ty1RUiJAJT߳lO69-֊X5ZŽg__ [ ta|+?I| =?ǿ>|`<[iauéh7V LJt &[ؿm5vxo#6~_3 ?KĽOX~#|sֿi WA4ki/?,W/t_FaZԡ ҜR|+RĶq48<+BqG-cM)a Tb卣Nei:Q#O" :)N%X)t\%RxәUCu?gcď&ZW}9h{Pn|r?6 vλ-í{O}+ƿRO Og ZtY<ſ,<[x<𗉼uo~ß~;/Ց|Aqӵ0JʸqcMζ34Tbo4fٍJ a*9WJ1X:TpAͪx, qT|&G 5jjZ^qO5l?o|b/.?xw5OV~)]m_\׭¶^ˤ]gK$zT /U?/> _>|D:x#; k>w7 '|5i'.?PltucEyӡMJQT0,mSU)xs?{juiԥY.g¥9R>qRZ͆|0>*E~wS }IM\alԥU<eڋÏ O}GCN־0O?KA|=ᯄ ?ᖾ/|_i~;O#CZGxSn^k3 ~?o~I~i?&w_xggZ|q}ᇇ<;k;△u:3V!^uMOj_ /譱:Xֺ>.!Lnu1ss~Xn-)%'* ^λ5yd`idק[%V4cNCU9j|+ԵwR*za<(6cSk6/oпK?%:?p)=koxz_ c֛6 m?9el|c|#º0k\Ϳ$ci? k^tZܐj`4yu (nR(UY;sc'լҵgT{{Lf?3J;T'e[q<ӧ ('yls xX'Ɩa4~ˡ9%(x[ro sL_k:~%ot׋^O<jNo|1~gω05_fO~>G7t >kyp_#^}?j+FS-NqqY(B5\JZ_ >!ƬzjUp wʮNRW\,B3r~1r|9PTiVR۫~̾ 'Aj? ֞u( "5OZzdҾ``?kKGf3폧|R[_ ?4gi? o ڍο`u{ _ρX4]6Q<ixtHzhaM\RUIβQ*r֩N+ޅU O$>TSR(TSMԤT]c:-9/A#/m&im~Zk~/"͇_@Y?if\m⏴ ̺oHG{v4jOS|FOa>!\xfst )|e?uKrsxZ nO kWwzžͥê]\jڭL=<:T2& ם"VܽB6pa2,&eR5sRGB pHYs  ~զIDATx^A-qnÎ׳ Ďݲ8 j{ngq4ܲ:|3,̆5q @ D^wo@ b;w?ӟ=ٯs۶eD!U~ @ |P="ry/Pۨq5#B >iR@B@ @(T?"ŽH3v]l̥D"G Cg>D>( T I+@ @*#ᣂPD#cG&f*!*%T=M/ @(;]wm Ħ#?_#$BO9B"Ÿ$zM@м @>D|U($cl)(mFIp'_8 @$!JT#$|]D"۩Q#"IYƈm%ҜBO_ @LW[i[+!{xJ="GT۲! K aF@bi@ Q]Q'$nhXYģaIb^t]D@k[F.^5ԴѴ!vڝW F6ݜUln`п]Ni:uI.j&lj- r52t3K^&yXV V+VWv#WIi0,+3hHX' ۔fJ'WrT3m#;30\N-[U{hdfGWiLcuJ3$?pj: 9g`㪻s‡;/Uz|i= O:B*vHUJH]e]_eZ]Hq&[-H=" A6!4~`Ke1ԔI qc֠ӕ4 @[wgp3l۔CG#X UW.-qt[δ4`%K#x""F=Ԫ1)IhؘQ 3!}f~C¨V%TKB'>˸4%2&n0^weYxhݡE.Sƿg'Xw(,=!e:ΰJG^KDU?GӉs,.[b>VH%!R\cS%b&`UJWzv^DBN1|(UTq*NeE|2% ~2*1 {@xɷdw."1B?\y׶T:9|Gg>†/ #s>Kh:W4_e۶u{9_ϧ!Ū Qwjib}2C`4IN Q"=ewƫB/$ ਪݞUUBK@@xkBw)҉Up@ OA zr㍹HWx }nɁn*Y*DA9Bk$ah"6@"1 <, eՍxzhKfbC (f[~d%nK(}fvbf5Σ'kFCrj;uә0!5]yu1J^= Ko~]Ȗ;=ҼObH_jٯyI17ָjؤ[Zڮʳ$R="%^s'{S]#FqA-$f 2/_B'_>wEij]*]4^:A\WZRQҋ~tڳ; F-^J{l{t:+1})̄O(`HOjX XY 4jXMG'۠OIT" !ҶbF/"+h Mړ28 =#{+V @ `2@K #3-70s1jwb((?!X*S+ߏ>RP  =+C9A1 @`=  8Czd'_[C~ ߵt5(,!@1GIBT߮09zDU%14'pUbh0 @@O@Woh @@[hȐ]+T!mqdU %֘HB\B 9?BZ"q$1)pJ_XB @0GdK nG8͌*1TюGHFGÒĤ 7T Ǘ. @  rѲɪD;G442O#Kp#Cy @!G4 U0h4_"fٷ2e3yFtU.d, /!qt%|8| @ % q8ZԠϲ&6MoTil(&925x1$ w=UB @q*IZ:{2! i#bRJZHwZUMgȾG ">b#sˆеEOW&p#_eC$ G /]$P/C.NF Fн{VCYL\+O:CJr|8\^ڥ,]WݺoRT%l>fBld ?M,wi(+w :J {@QVTs4]V.,ds9hXpy+# ~䩖zǾ6f%A{l GrQ#D2KWer9KbRPl>IfL =B (? LFVl*i>~=^^(R>7^I1q ռzN+C 鬋\m,yZ f#KW,B"2JapMkIJ ן}:YWդgWlWTq9Z f aF]ef[߭zKIfYGYU)jRɺCAj"WZF)NU)/SWW*W@Řr5^-dF?kڰ 3 ti_7?s޶ /TյbvS%d|O"vE~f.5`6NzTDc]+'dpwCѬ;YW@F/6/4̲H_ /7M MxT2\&9ƾb/o2pٕ̆P/ʓ[*-Ygx29chtjD4HO, 6^o4Sd4:RZb߬U7#B(Mvp\f= |&JY~{u혋fQ6rn7WCo S:LvԿ!MG\%0ybt&sYWw#ofcKG4vjGso/n7#P%4 @x7v|(J6b8 \/؂J~_㴥iegՒ;F xs@3݉Pjq(jݩ;ߣKJ\t l솨\(0YT% TO6tUՐX9";"ƟF DrI cg2B5 hj{z)C% +oƠ*FO[@x:*4Ud11_٢}E XYw{_x|{fZMΊn6mY\7Q~tIec3u_CHWzУ(mȉpȕ*tbR??5PH: qGZÒKK$؈x#D"^'5z!߲N!{bC=W9qhrC4 @?7A" AiQI @*_51ZH#N M0r~ M g|KT!8G8sR @H@L6="xK-c<A bYhTD6=bH#/QzDW)\R@8NT H`4v CC 8V%9gzw\}u ]T ݊Ɔq # %Q&Fp#$T ۫V @pzDCZZ8̊}m׾k`une8U`"Ib~0d1WWKa @ /="=>)p<tm5xw.ѕYw]߫$|ehEP/,!@ 1bXc(5t >#4$|oZ|6ΰL] ΩPf @p+􈀴]pMDZ)Oi[M T~~ J[j8C҉&U& @(aU%V G J7q_g2:JZSG mӈA;И|v9ʤ- @^L`i,=R@u !gvHd6qG?qƦ ďRɚ#ST  @B`!0u„ٳ>*6C[JU=bБ$\IQ%!(C0 @ #T ~ P)Q,0,MW@W_`0H} m#V&\R3 l%C۟F^%1WFH31(tU_W ?1h@ K -P-@p DyA"Ows\-p 9 C"A6kTͮr%rTD|>g#5>k)YCh8%P$Z?51 : 2* o @8@Y#W%!!jYC5?Bɕ|pyTyoϩ$gQU*I\9؄u[-%Ř"qW7nشm?Tz =" vpA @"P=߶w Ű%TJ_A.ng)dV5QaHɰe#;T"S+?o ND"_^wO-+_H=W 1BOQGD3T´ @T?.]CL+L3!,o9_#q#|_243:Yb!Ijb-zD~8ĺ)g@ BG`&M[w1Nջ⶝NVWV1[ Qs#be,"ԐoeFORwIbHX!Fl#bqΏI @FGd;lD*J%"*ČrܷiCRa_C~[!)c\/e\iɯ@,Ep?{rH0HwjFKQ% @RTض$.;P2*z%qF&Fd51;ϒ )oE .F|γgjdh(qKsT C@ E`HJC4'.vwmv* 4Xd]";ܡ}*DJU+h72ue=zDSCzGdXP%&/4 @ ZNc64QO1I4XQƱM(K!zD?xpj J+@ @F,FU?ҷ}?K/n׫e0fJJ>W_GOӨvZr2$Gql#R;GGT ۴ @%GH WR_k1?GӼmGuQ#XfGBc IbA#DG:?UB3ϰ @9*!jjak1?&mAiK1"Ep$v{+W@p8 @"zHLg M7iiRl٘瑮>3"r~FqhHs%&g"!@ =BfȆ6k&al$q]G=B/FX"ϳDO+-]W=#bR=B2Vbќ- @\#$|G 0 G97y"O#ʠ?GZMrI]Ћ7Ro @3U:_a^r>J yph3 BJňzUi{8ːbhJߵ~-ν5_# AJhF @ H7kG-H;ô!eE:AЀ @]]=BBv1*INqfk]#;5J~C͕P~R@]h@ zHލ D|b-/î*DU8V׵Q##d N*J!^P+Q/< @0#DWYV%VJ5ېYuJhkWbz`3Hf=EؠG4Ď%qty Ǻg@ G=̲l\Vկfml#Жoڪ,(Ǩ;.WlcaX#\J∪"%&7~ @A`8"[WnMwl̻$c% Ͽ 6߲3UU}D[@ ӢVcM*+TjQ&hZ @p#^EEwG6aqhT#z?oĆ#g$zKq:=[q%X,#bz-- @;iTbU޻y mDlFyz\i% [q rNhd7&ͥG@ l0$$4f4B_E&0J@aߤ+Y}RN}="GȫJ#ňF;)hOG\bhd1 @x:YCd*ѶokQZV%Dz1B:zr6% SCfJCxub(I!@J =Cm)1-CSc@ Gzg$vY$ %G+ڣ3Ե~\>Z=";R\q(zWqE%kqӱkMxm$+c@ \՘Kt4461`#hKx @@@ЇoNbs\.rZ.,=btηΝðΟ|F: Gq[W*01?x @" 7IH2n .^RC1.hQ14X',I.a#FCf/?iAZP=8*_@J{Q۳EALlˉ>^Y2p8*I.G!ťw'ݹ{iU@ !]=BBU$aHp#F(Ma=Q*h$Iμ|(* @8@ve7Ja@ٝLߵC?]·w}bm# #wl+'G}tq/[6Fnk$@ Gl֨&_UuYi' Dm2fQCˆzfFz,KR*jC;#F/U*TXB ' q3xKkf,V'_9z+Nm0Dh]yE#BYD>bt,_+I(W+#2_5#P%c @>8"[v uכ{Dȷ۩m.-mhZEU"M?i6!G_c ϒPONOs4 @xI=9tE(F1 FG UfenFN@9V{&JUܲFΙ{Ƒ^ @ dqjysw1Eu_5+#$>b]xIB_Wa#@T-[Q.E? @5\6kNc:k]ruwGU" Vhć}߳ecgItNFp,U[ł7μ<L @`)ɓ,5`}onc$=z姊k)AYT eńGbzmo\%]/-#&ǩͿUF%la  @7kL>w]U5nޯzuV=e D{YvFmc!زQ}C 3@ w7aU([V\/ZڡnNq~DW@XzJ?C6)D!=bP.13=h @G -(6A؀kڶVϤ*kl9`#?u]2JBZ9/VJݩ͉U5+=bj$Q.;@VPGdan d<yBgd>gb#DJD F&I#vGʕ|wT ~ MaE]qN9 @F#hgC/aqQ4sSt,"myY.aAGe1RI5aI":վ\tC+=B)4vh_fKsA _mL,F|uQ%BGde =}K_ݸ3=B2^F_1)%Ҧrb@)"]*xYNwI@6iKGT"#&Ϳə"q!^*K&h_G_W.[-aB @ |qjUGDaSrS{rJ#"#i_DȹJzEFK0H]8-K2.a紂 @^#Ґ4Ve f}x#*~R izD,J2W}$ڪ\Fվ/FF篤/#*%7@ $^J4#Ҿƕ,WV Vo !FT"j Aڷ!oC)bSpɣ\ @X#|'o#zD Uj[|T̙dV%eݠu13(nw)On KƘ y2F~9ez`tw_G C~[9UwJoo$yui[WD*zl֠>;c%IHqzU;5$*Kޡ#|/O{t09W2 @`?S#Ǔ0r_nOFUweKX_+#ڲ_C?$jMH3mr#%=.E @SzB#ܟwѓ#ͺ ]ts F]T$ "_CeF0yixG=A lDg.PU6k8i7fH&J1COW%^QKI="n%GȆ]ya5fzzˬ.Ka\̻\6\ݤ+E/$@Rc,f (MG܎+n.2TD'k)2'񀉻T (Ж?5/s!KaFE"B?"6 "M @dgj UG^+TIUhk:3 bH="-@04%X濹Vbfm5lJJsC,].F.N4^&>C کQMOp~n JĠGyz#V) # M~tFeCĢҭ,%3sx_ml]\fREgLJ_H rF~Y@dE٭"mH-zDtҨzX~$#2@KU "?gM֝IAVnC#L('6f |ʈ72zxR؀7O/FjYU%DHňOkmPr1t- MڝW:ya@ J.;56p}04rغzתq$h Dh~E *A)%̫% _ eyK MhL @x ;tPHl1GЍf4n5'm܇rHZ 敇jURhڴv=jeq3ټ0a4 @&wj$DnȪ~|HѸEڬQ"u0]omz\#ٯaӏmb$O̵Cx- zh^U<]gC>{qDq+n{*)FWѻifHsmI؅a*}G$ڸ!_7&EՈ:Hxj2N^n&_-ݮ.`cW.@^L`yV#I8( w݆\UBQǰ1"DDU,&%Qm¬Jx-WR56"OmL6r]D&*?S1W.҇ ͡҉aWH{.i*GJiiwtFDH`Q?GI=B9jEe+*d"3s{SRbpq]ecw %YF;T 3 V\k2HX˔P"j"\YZ˩fKe$]q+&T u~u8ߡG.1)*+ݻ[ 3@JS']xi*W4 Z)\yPRZ IXHr[n4?$*!FJmBJ/In O#8 oWՅa@N#86'F֮d5`|v#vwZUG} (_H UBMhTGmzܛ%ѷn" {fo Ct\Fy¶.-_ȻZU! 7ݤݬzRErwt0h:G=y1k۪="dtՋwڬG𺗤ޣnRCÊ1 @ (y7?tBk}՗&{R+R||L Gh|G F̌m}SaSi. *84(0#~I}~&`$C RU"3*_S?)=ffM$#kf="}0e6t @`dqDWYu'd\_WChl8Ym#/#%_ k6sA%Y gI OgG(u/z]W]}_J[l?[ @ r:rV 5| W4kҶ 1~ ߲8C B͢*~U" U"4 D/PvC?[ Q"L> >bDDZj Fӣ>~-!ATLWGQ$ڋ*Q-R%!1mP%z()HK°74)']їKz6K 7ZA pE2yBWn\4)`@45~Fm6*#d1( =btܱ"P$BJT%U!1*br 14xb"81$gElP18d | =vB7&gD&mzT%D="n#4 $!3UB&F3%\.ݬ1GX~w燾Seӆ^a- @ d;5葮X>jRHZꓵ–_tU%($FDCax(sWzmKsI$@N&pulRU]]c(!69u-bDCD#@@#ITU y8hU*3Q/y`h] !4`@p3,׭9;nq%f4٥pc'|8"JS9ғ#D`DJ4T =U" P3jGlXu7d< | @xRuW;mTdN9g\6qo#EG-A Tĕ*+/A;2t!b#$=bt '5]㮁2;/?4] L( 3@ >}ٙަJTu>1r[!٫8mzDZ( #n!,\>"Dž/i9à4J$G8Jr;d\h}@31N\h5M68=<~Q{CHۚr#妄/@>heDd?w0<iۛ4Lgwf="(]=brXin&0&ITU y2h<]"=Ө*uVPp1?kˆ\”b$t)qe „ !@ `#ffV;u@a|&sl(bDY/Q淿ACCc7bO6g[9?\IkhC~(-gt:U2PJcӅAp2Eqt@ mӨVi#2uh\)xPvLeCކC_"]%" p%zЋcwFIB:Z"{1&4zDk$1zPJi֘W=HaJC@]ňx{P${3Srwϼ01]-NR XY#M,#$Ja]SW/׀tstB}4_Dmb! Jߝ%a@1ّFk fϨ}ds۝|~zDYQ RbD0CP0[G`VMi0!f҉rLvrDLQP^f^uYi֘J$ L< !A67nJ~l 1,  M6u ɫ#JD\v5OʗfK 8H!x_h4DUe;#Z*ik |5FoKn7N.ѧ  @`?xת,p*ts91 J F?xG4fTDZ8;1LGU<2^JHW:2U^="+I(/^Jtkl|1*/U7_ %UF,%Kf'6a])L(i5ټ7#iJ#DNU3$&IG$^=C[V:"I5Bze(ġѷ[V}rYV~01Dc@"m1{:SBPf4k8jPg:iݣF= Djq׃?9Rf/OIB"Kk%҉[Ĉ;zD6:W^_[ds{f@ ͍N&| =(ҵ|W{GHqzc =p dr?k/Cf#ɋCetQ<7\M;p?61@M@|X l=SrwCY#I2)]r$>v2fNL FoٻA}! K!JDY [Jƍ6?j5/g4tdv{A 0I #Z>ȇ9Dlvo1x>6) 2="8 4C`$U(Y!Qp߲t\]H3L67Us"i'5'„  @$0$Flx{{[.En/+ l{4s)HGhcsUDT%D%Fk%!ԤVb[ =wemIT%D&Fq)+hJۧW7 lq%@@# cuv&BSެQ]8"]@4[%Jg*!MY"U% UcA\*4;t׵9-n Ҋ ο\ p>rFcd:+W]45bh"j]0jsգW]UBVL~f% Q%/EJr]%ihJ$6.;-y_ M  t<0B3(^K_M_͍]͊6Yb_8DD="a49 $_U%\ _3w朹E$^("@8@B+ݕm@36';[i6>[=ʉi+sGD=Z14H6IJ(LtjS%VWFkgvʾfW}˺ 퐢a@%0Ta^>K6';[G*LkijaCc$JU"1dyMY벉ㄑmU>p2{3444qN4%l@( >GC֢V^~4]hlMdh 6F 5҅~SaHqDD bv>o'<$Fo$!_AVH!2a?3K$4kyfiho,m*%'G! K|}Tr;_9كrDf6FN2&.)W+XM__F1:؟LIBwT_ʳ9ռڻj="dqujm;d?d>Va-A s%f\=IdeJ64yQUYĸR%Zu' B1"7#oP[C$Iw0xa+K'="d7*I,ϸm[/[5%y h@"o+Vѧd3GێOδ(ITě:E*DWYW˖ C-#Osg8KD!&MUmyBZn IG_3a^oZ]?!s0q5 | G1\{ufe2mG/۽SU1 e#)Wݿܔq{U@0OO8IJv"+y"+ؠGִnW(_<ݎO%v a04 .{4"ցCKJMv.]']R+胜wTj 8;o5SY#ĆIS(I6f+_/yJ|1& iy:݆]kam= ᝬ;@dlME=vu@1yRWyxD| `b,Jx)'\1=^,zƾZsm>Ũ^{9rYE8yWC@ACznV%ߗ546;adh(ηգ y' ]e"Fd4~ura< )Se=|2< =Ielzˣ÷<L /y\=|Fn0 -wΚ-x7N FA@Ќ;6 p$UB CĊJJwtd}M" hO1&&v: JD"i~ ݶ(g Ic8jj 7ޕfz wb!Q"·0|i}D(e4y"% aZ-¦J!EF_z_Tw>}y4^Voɬhm! O#.XFN{qw2UJٻȢo߾e6UBĈ(@!Wx$R.tUE 18YB}~,2mdHl2%bE֨+a̴!fCG< ٦!YXGPk D1DT% &Q\JĎ%~9-eGJ+1dYlx3 41DyU%ʮ}8X]cdYPoH7kqHqĆA Yzy$N)Q:uY VQ}pa-owLT1I_+[wV[JRJ& M>%  U"Βn#ɽT ޳s]0 .n3>".>x=x2j3}:J%"{vy#r2d\}gNԏIƉB:qjܳkJܬ>b=!=f ֋yyiNZw:p s޹Q1J5bqD1<'i@C% J}3g^fj]Ģ5"jCꣂ|#Q:m> 0C`֌L{sʆJ2G\b$~I=7ՑWwv̎ eM.r3a\?qi}W 319 *1SQ*=bD#h@Kn%1'Wq4&3mG=o<1x4Gz|܊wT}+j"a2 n?&#Dmܯg8b$K1 צ(JW eehOZt4X 3$ @)C Q`'/ 4\yY {:mm|.ӃS=+| 4 ȃE7="TMY~Ghxunsɺpp;Y:%51xni6{}y)hg]]w\HydO./)UK.>JWR[jfZƃݶGҬ}.{-oP5e!sH#[K/,!0CIBK/ܻZո8*1jYٸܲ4޽4eْm[nc/^7sf^lOT&ff4i uЕVgwm?vY Ji4kLyCT뺻U|-', @KT_64^4r oYٵSOPAi6yEo9mefk&TCO j C P+2ЫjYRJwjy9}py ^fb^iv*1#oWU܊1ߣGp!$1-L(UjE,p$ᒠƉƦ:3niC1TᣥWdlh=.fII!~` Qm3$1/|C$W&ejUvh+Ҭ1p/[Ĉjs=oC !EIB2\]lWJ&R̬4#ڷ\.p']Tԏעlnc;Q4زRoh%27( ji۵&AkM`Moi56d]\R:_]5 y _Z!ዷ| x<(u!<4^|`o6.ݹ8`n JRKsTZJ+b0 dn$~PB1 "zâ l>fx^mdGx1?^x $ ;(-&D&B".cEhB{bho4, ]:.+"}ɤNy:=hU9jyנ;g gsC 5s. ňy%B8i8e3$f}xɅ,>8 .18&V];`h,\ux3,FN|sϊJ=w G-绞0~iTKWʈT??ˊ#!ѣ8B3H-ƥ„\BJ$Dfˁ\i׆^4]hlqOIYJfJ#L%]+׭2:?yt ż}#V\Jwj7U"-(o)X:mp!HC.Eg%"XT.rh9m|}fs8ttUD^JUJ8m"-d\%´r4iҫeգjM1ZVizȼ;,G(Aq$OIb\"&9=4n>fІ.f{P[H`Sn"QOΈ؆\Cn}HSSlWK(גO7>h<zIK u~O.swoy?G{Ƨ{G+<@`$rL%lzDL.Jѷɾ&+bdAV#ݦ!G8+68*;WOV}ԶIEUtuzdxTuXoFizQ̴>8LyJޢ׊Iݻ9kwVs/˓TۯEeخ~:{Yzq8M[j SW1 uC]7 GKFv+[-"NQ#Bw,mWCIb_"w;O&kYwPO/ym,t WkL+ u1d]7FS?l!e޲W՛Tn#8C8"7q/Ӱ8j+ ,./|7\wZ^!p5]5m56_oڷ[)zk@+4(lW~#b03N ƍF!;]"vK9"*}^7<3~^p֌2FKi󸋤, >P?!f} K7.u/R[ & }uɻ;lXJVuNNp,Gtg8UwN,ʡT%qx}`gjt$}f",46դ q=6zBiVkn]d|"yTءĖwyx%h>5^q:NWGPݱsY^;C)LغpXIo|_.yQ\vfA_2Frڽ/UvGD`$f8UwKId@qE-H7 ^;yJo p3͟ nN"!K(fC#ޫy;XZQ҅Np4nwDPt(fS>PCyL!p2$G'0qz3п^A|^28?vU밬;|ЭB-כc%膗I1gZ5CiwU M! GwF*р3)$LsCI∱H%DM-$uC6sx'7<]ɳv'hzBb@\bGCrs(Cas߮LF%Y#DH/qz(U!p2$F' r`O*L^x#Ww$;L[۠ 9C2 N,JnN$+nWW cyuȭM;57P Fx!$qX|$\jEhAHM abm2j?:&O:_v]:d1.fWKs NWsHW41P"VFdn)ͅNT5~tgE4$$q⸤O D,0%sq\<c=ⶎ΄' 7tUMl6ԑf2pWC֋~ j[-yHDXNP\1+0bqc8B|=2"|#F8$$q|E ٵMxT a"X&;y[zvS4E([E;}rN}LC {VZ^)XScP"۩"!zĆčU]>*Lhe%Iq,ʡ `qOGzYŌɳ e$-";~w+8鞸C)pwC&a*T5Ї6Y)F\[^wjde>(G4x.$]#VLfǕ!ɗz0!2p&UճKw3)̴L$VRڌ4doh4 _c#fzYYDaK[<cF--„M?ғ/]zsLC\m࿡Ș 8C">vPCUc:}_3IʢЧٍklia؈OyF|02 !\HOX.Q>4&%bx:`Bn3З:o6ߵƃf`h sC=vЇ%V#^YJ|FGy {ݒu;N eóR=gj(Ib@xހf8$#l+hCN!w>/F</ ^~&(Cx] }`WzrlhؘSP:w#xk*eieGGx$b#&@cɡ\{<='T=`lQus[NRHK%"#xs@$8jcH ߣ%s)˼ZBoimQc tod];=^ _jV4?yfSn.GK(.aXaTJ=f4 I<~48bV^*&]DZ x=>~N'.18*kQB.\]2Z7<[׶V`1j# %vj\R~ jCF{"@)$2R8}|,PMURDbf>,<*,u>31ni[xbΓNZAtw8 ds82gɕ~#r%FuRK6(FSC?RXB$3}K**x4ћd\Ϳ ] s8;:j|Z05{[ɨ4ͽl6`Qbek)@dU* %ة8 Ii4r„G/y$GlM;aDE2RαOHrTwi^kwwD@8peC#(QqgjƋ&H8D r }D$(D*e]7c869۳w_eqZ<}j0:54DhPclB݀V-#*N5vj6X N VHoAHUB䉶0W%FE~z2vQ_4j(Gi|Z_O&gF!q.l mҰ=DoWE JJ,JN<6k9dp%F1c|2$~ !maBJd? ǩeYxExcz-5C^#ǐBd9Y8-VmT[yQ3d\iS#ş}4q$km1"l3/ $AP}PhZ.8`K$7„˝h,v lv`.dІ CMa tF;F5d {~Q֥>.+1b> X!bFC> wy}zRBsp~J1KFV4N+E΋ܖYo R?4|-M|hk( 6@pq2?a2J1S9<7 8&`w񦕕H&Q߿כVN/VC+0hĐ{pQ$J1TSCĈ Cȷ4$OUa"Y& Rzɗ떎<0NnO<{zq|'l4|#n.'P7&%}MHC0n'w^`E3|+cX'dmb.#ʲ1B?$>wbdD#@)}D"2R%& cmI F1ߖuu |s]7xx+vi$lb2xlDV!cqf$ɗ&1„;;G K{x>yM5DpAhP6$O&塶C\""F\JħbDL= @@ze|GDouK$y&P)L߀2{!H%sR>'lN l~ξz]\43γ!&3ʹdӰY:ϣ)Mʐ\Ĉ  i1btp$> QU%B{NE0s!Q;gr`HI]^!0)p21usc#ɔ6,&z 1"ާE1T"$) . 6$ f/8ATHĆ]U"Xjֻ3oo Kۇx4{%7-icP8ff!D!'#Jq [Gv*\"PFh&+]m[hO/Eܾ |pV({WRY<Ɇ.̱ 3N}eD2t m1"E[@Wq}ar4|M*2| g|nh+RђDu൨hE\cҬHC /}eBDc3%kqݤjxFվk܂{9]UH`hi=fyuOyhhSwރfx5~#ej%M?{YD4*JA@`U36K8BQHOBr"[9v]X*ޕv\l#fpiKUH)ɟq]M6,9҃jXgeP~=j8=L,!IH?y&LtT6IXG>1:>I2x9N^4ϧ?gNɺY2]'^ڮ;jha*|_A=t~)gF/ \@`nh D&9&T !"L,&j)*cqXrkՐQF#_ihtftg^ňƙx)N t Ita B?H3znH'D䉨MEڞ2.N>-_h6' &slnxC%5ѐm82ikKm[ .zY{)Fl,1—9 =$ =+,hrD\BJdgaM܅u⥲&5:m'v=vt%6j>|?̞|Wg:bgcdĕ*^/m@6H6n"P&rgjT tQvQ=+ lZY X  feO$-jGeCГQ炱MCK@`$E`?0!8+RW - Ga5v0nO |.-|hOkQPiv5C&<%v0d*9'OobG@^l)LmaP(r&z19,|M4ºߵK.!8qI'-N?yYĈL7f"H I0< DaBM?CCZ1QJ$bWZƨ0yɡvd4KL,(hW!82586ߝ6zQ*|JG% 6QxË"LdJ4D3 =#zBF1(=(27 lS#`ERDiIx4$G„-Ra+1*ws[9>-]Ɓyβp2;qg_UP03| %B \Ds=mɦDsdZA@gu &D"T~;Ú,46vأڿ&KihVO ۜ W]_n] <_{CS%bs= "U%1B3N#-,Ox^L۷oA>rL2  A25VIeaֱ+W.\8\"[[}+D8d?dhHy^U/(Cz iʎf] uhvm8oΨ\JienvV2kH+;۴7dpЗ蟋6>&rō$^C P%G>#=RN~᥯~o&4a@u!<jBx{ܰ8uGT%F)6g2ea21"S"dDpf< p>$LjT"O -UFD;9)=E{n֫vWf(33]<*PݰA_̛ZME(Ds=CRLň7q&@mH̐'Ȅ\"d"IV"/+jvt ucaXyo) ]9y-H#dkFA=i<ss#/켉Mb)MD*7k%džC#z}[ :JJ?GCӢUցcnԷ]Z=n p8ƍi›I_ȷѐR%"Mʈ! tHOύ?&bDT=bPEð0YŐPwd12XG _]p]mOqE wCyV}"Fx%oq}&)8df%jEF VHEmً{W;w0_Vcf,{!neo}fUfeLP\Ϥzٙ)-鸸j8q]tbEФbo;D#3̓~- J}cO 8J.֝4sC{};Cڊ9OC@$=]&bD#aBM̨"O,&v;Aqd~s"ZfJE㰾UQL,Ǭ,b@bDc,HvjzGIqCFZ0QOCCޕDvD֪<Sϫ.ߕs5(̡Mśڻ 3VAëPBXȚ[B#$!#1eD>>C~VL(U,DibMYftlϰ8ܸRe4)˰} -uؘnZ"Gx "o"X K_}8^FIeJ:u=J\=Crȳ6tt%og_a bKy5k% t&=)s+GslE2M{@HIS\"  [Dh8C> wƲ{|cB:.' mxtr0&uT  {7h2D<*~8 $>a1' iD&Ac7 UB:бh'%CQp( ƯOpn,Ϟ5WF7q~Ve(m0aRvycDn٠/DpAYc@ %$|\W9>RHw^ծ F8dCMNg>3O\5kgO:*Lz$$NɌ4]"ʎڅYYDU7 q@D'PM8Ra"͡ 6t۵Q x:mg_^1{!K>R sJ83q"¬1BnGtOh| !m$ E>T'hHYN(K$"*!M ϰq6%z 3$H64>߿uwu.8D .#uD:fJDl"JeKap<c@C$paqD_wT~()Dab*o rPmQ_n$\tm5YW87*vfݗsI]ڵa,wi]'],D h 0dz_fo6;RWƕ !1@ظ1 /'FaL[ld7yL؇t q7p\kݘ~&t7t,)ds2d$=V}gCGcϠ  HIxf* zmbRH|ґ}Pb4np/mWw9̙#s&3ƼwJJYH2Y]\iR"biAvPeGWΏ5  Is D!-L?~`v)UlMt#Fl7&k0c\+f⌫bA6, 3fm]C X[s| ,BI 3@@;>@jDH/ [ڄe,߅Nn' (pK&@H:jl8~B d4/1?)WFNL o $ hbDc+)&:&U+6mBsT{&^.>~x4 펺Dj?&PkH1oxω{@J@@E }&hX&A;o~2p>h#~}cj.VIbwscZ9ѷ[j@ *Pv=¨}w)ce)5en3OYDw|1 p#$nO/'M{X1M( mgwCev^}N~9Gx^ڴM4~3:eCY? X0ّ4z j"b5D|Ksn@TIL/" u;״h"~Xd+|0|%.P"P?J7//oW~\ M`Lz>PCPhtagYHg(`T]iKt67d"]T v [eau&:gku{4_=2}uD+@%$Ё#&&j}ܒNlk$c颫!UGJ>8ӵrqY^Oi׿2DrsTP%Lom_FDGK@o"$$#pMT@'&{J^)&Yr:^k9Вw fuH|[)u:BqmdQe{;D8c`UMAYzRhhRQAD %6h@$:Yĕ0QYTކfXPS1v1wIu\r+Q7 R6`jcjaנ=^Ź$z}%tZg$ 5\T;5 ![G>@M4qބgX EIbh;%}5vI'qXр%Oh߭KUc;қF lQwz;쪡>(~ef+0@.IK#WR D|_y5ߛfb- + .0qU: TR4gwJQXybl{^üۻHXc<=+ 5R *3uAH(C@/&$%5|H&HV1!(LCU1$Cͫ8i&-_G[.@OU!lx2DV 0P" !ċ YV(LC(Rm]:1$+#͇3(O]'l,3$W.t.}GA[ r$? u'å%@hxK>3DS"XI=Kkzr !8\}"ŐWxh0]9I>#W9v!@yAp"&w~ ß9PW%4lv agAv:>5B@&E2mmBƐRGW{;xQDj7: } 7$@"-}E"7JUNa)xUlOy5^(ݙ h Ih(a".>R4j_wɑm{ yiCVI1T@qI 1WbUK;R&K\ee:桹J%5ULsH{^ˆ,4D[[(1WFADV H^Q"lV pEI 4J'„xJ'jЇqÐB$;$2°*Gfؗ4b5K*CT #2*IENM\gR.x=G0 3$ 3:BTK'DM帵zW߶Bhdž*D3I "?W`sa,گ8C)jU"_=;`(%(L(EN1"ݣ(.IDk Yk|r:oe }$*\ө!@T_U3 Ō$ѕ!Ҥ /'Dt/@]$"Ox9;ŠʕÐ$|NcP7 eZZ"qRx$bKX$|,r'E1*@My^ף Ő*1$CdD'\X [ I܂N!ADvD +8`猦)9t )uq%I0Y +:JQZ"]I" Wu4zA EWP=Y:(ICH  F:&*Om!L[gHX1!$CV#6 EnSeD+ItU(ĕ ~#C`@X@@[h<#8ޣ,N8˕fNew+5jI".,Vdg6VwU!{̼WKRDj`Qw b@@8'p%OTwsd82ybf!6]~|Emk2jkf6VA Jz:!-.H3R>pVJt(#Cx@@8g,"P'EU0?G]]$!-,vImۖ$R&܍/B!I?'Y>a[}9LZ(dI1 <ㆌ!A2yQ4!DD#Dx ٢)ƁՎ\ݰFjT$d,E5.՞B}͒h )& ( /RԈilhHCc@x4$GC!#"X4!C[yKV.m^5jWEЇV +^D!M,:DjR:(I剴6sR3+B! [G _<#(6!E͌6ѮqK! sDa"N;Y=NUaŋtB $`Ő 2[cP%ڒD҉Eמu @ 4xhqt%Fq70>tX&F'M Z:jJI" O8JJO !#4$ P%$Ā>@UO-$>VFQ$ v煉r"L"ú=Kʩ r鞏0AM̒DCث$}ճCEH< IxP |S.?6H,1aj(L!r3N7$/z΋/D'ĐQ2G1y2  I8*K-O\V&q"J z+Leǥtt&]|ThPDu1E?B*5\bF, t |W$2m"DڢCzlJukZ1  I6"@v|4N ="/Kֽ_* 4„W/c&fVy My҄$!EDe݇L͒y\ !@'$!&A@%WNtPYH2V/I"VM8J?z6 %6|ġ "=hb@x%$W+IAJqf c nTmBZ+!#  +$$EIA _ϟiN0]|e!@#$Fha @4hg6$OTӘ-_OdLn 5m ϥ$ap"MZf4 |$k2 P!-(t剪$U(aHG~rzftՖy OPmD?f T I01 @@[4DCP.6/Z+c5&x֏TK*/~@%$q/z lJBsRf,Я"^[*(JW~qP"  E$-  T.2d"dTeRKW*%^u <ijƋh!|mCDw2ҌZ3:B p8$ @` w`8 W@x @ @T:50 @ @$<@ @8ıCC` @ 7  @ .o:T?kt @ N?!lx4%+@ @$ ƒ @ ;Ǖ @ @@8| @ NHW @ NI"<@ @; Is\  @ p8$ @ @$$q%+@ @$ ƒ @ ;Ǖ @ @@8| @ NHW @ NI"<@ @; Is\  @ p8$ @ @$$q%+@ @$ ƒ @ ;Ǖ @ @@8| @ NHW @ NI"<@ @; Is\  @ p8$ @ @$$q%+@ @$ ƒ @ ;Ǖ @ @@8| @ NHW @ NI"<@ @; Is\  @ p8$ @ @$$q%+@ @$ ƒ @ ;Ǖ @ @@8| @ NHW @ NI"<@ @; Is\  @ p8$ @ @$$q%+@ @$ ƒ @ ;Ǖ @ @@8| @ NHW @ NI"<@ @; Is\  @ p8$ @ @$$q%+@ @$ ƒ @ ;Ǖ @ @@8| @ NHW @ NI"<@ @; Is\  @ p8$ @ @$$q%+@ @$ ƒ @ ;Ǖ @ @@8| @ NHW @ NI"<@ @; Is\  @ p8$ @ @$$q%+@ @$ ƒ @ ;Ǖ @ @@8| @ NHW @ NI"<@ @; Is\  @ p8$ @ @$$q%+@ @$ ƒ @ ;Ǖ @ @@8| @ NHW @ NI"<@ @; Is\  @ p8$ @ @$$q%+@ @$ ƒ @ ;Ǖ @ @@8| @ NHW @ NI"<@ @; Is\  @ p8$ @ @$$q%+@ @$ ƒ @ ;Ǖ @ @@8| @ NHW @ NI"<@ @; Is\  @ p8$ @ @$$q%+@ @$ ƒ @ ;Ǖ @ @@8| @ NHW @ NI"<@ @; Is\  @ p8$ @ @$$q%+@ @$ ƒ @ ;Ǖ @ @@8| @ NHW @ NI"<@ @; Is\  @ p8$ @ @$$q%+@ @$ ƒ @ ;Ǖ @ @@8| @ NHW @ NI"<@ @; Is\  @ p8$ @ @$$q%+@ @$ ƒ @ ;Ǖ @ @@8| @ NHW @ NI"<@ @; Is\  @ p8$ @ @$$q%+@ @$ ƒ @ ;Ǖ @ @@8| @ NHW @ NI"<@ @; Is\  @ p8$ @ @$$q%+@ @$ ƒ @ ;Ǖ @ @@8| @ NHW @ NI"<@ @; Is\  @ p8$ @ @$$q%+@ @$ ƒ @ ;Ǖ @ @@8| @ NHW @ NI"<@ @; Is\  @ p8$ @ @$i @ @~פMɼfTH @ zlx  @ @DH' 1A @^OICL @  I8*@ @x=$1 B @N$$q @ @$^?$@ @8ĉBL @ ?ے[١- ZA @ DSD>=≓!@ @ m@?x@ @x." @ BJ @ p$(x6 @ @FzčG @ $#;D@ @$Q =D @ O.ITK$8ȉ @ pC@?  @ @'0J'Dl @ Oq܈ @ c I;4@ @x3$7.A @%$q @ @$<@ @8ıCC` @ 7@x @ @XH A @LIͣKn @ c I;4@ @x3$?o @ @E0wFC@ @ $  @ @It @ @s @  I.!@ @$Y?C7< @  Kh @ @NI@ @ $nN @  I0 @ @  @ 1I"$!w1 !@ @p$@ @F $P?:ڥb}1XC @p!bs!WZq#?Ki&'N# @ @$!2D#~,L(!Uye< @ N`@(WDRˮ$!R!IcK| @ IDB3\7w<#e @ J`@)\mX]Jo\ @ 2no-C @ '0,IbQG/+` >@ @XB"I,  @ |_}R @ @)$N  @ |$n @ BI┑ @ @G@&Y@ @)$N  @ |$n @ BI┑ @ @G@&Y@ @)$N  @ |$n @ BI┑ @ @G@&Y@ @)V%g:IENDB`PK !*++ppt/media/image8.pngPNG  IHDR&>sRGB pHYs  ~IDATx^]^Ź˅s\pCUR)㊧ M!# e4 HFBXG3bO S TSIH\q*n lQVs\zƱ7?~`;??dۙV䠮 HmsҖ*VS4{'oLPo4+M]Ru$"EHcE?!LmүS:[uOl)o[9&خި6^Y}`'Y~:>?k}_>#/0߸v[[ln.#GNgǖ{fy/dG @8Ɵy9bm9g Q I!@ @s%JX,\ ?mO2>uuRSyQOڶ<><9N/U~y?߯_h%w.K|o_GZ +Qp V34lszZk-֩N[)0q7H+l p#By zZNx?hWVOO vXV|Y^#}k 쌮}Kn#Wϯg!/~7OMx˴u񝏟_}' I[;}}t*܋(D1]q7HbڟLZM4hB0 fvAܓ|)eP~(k>6yY;q*pUM%EX}lilZ}w}^z}*/UI0W>ZO`PTQ Z?!VPgG~#̺IN+|NR"@OQI{0nu#,AWU≻~g GmƫEP~SzH(I>!~*j.f&dGAU2Hp]#ĞMXy4n֦.;FZGT{Ӿu?EYV7ҥ및|5-w<ٜ%ӂuUD4Hseweaի^յ3D~7揬^*[.\ ]Ǘs7TG*LUBykNJ0UuoZ#*֨VvtӞWkFdijUzk WIB 7:٧zzO9bJh0'nt Ar, z҃ǭKG*cB&nȫJ SӔ({ozO,UBwl9^ziaʋWoFT =f>RZ#ˈ[=?#1M8V֛?B+~YG,U#+QJw,-azQGiqƕyaQ#6qYՖ >.ugU˦ð&FB7J֯RU-0cy3"čk[:vgSG47EM%1TTVI>qUR%-ZVD|JMUBSKPREM.oi*Ѵ#t*Q!| lԿ֬}몱eč0$|bu/9G&$Ez-nI3]inZݨѩzQ ZMn|rjZԺgܗ`#b' %֟R4S;i_m2&.$nZӥċ*w@knuNGuyF JS͍I 1JFl8I")! :^ǰ>^:cjqݶ&nۺ$4ֆj7Ψݨj9@kD尊5P3DtꎺYT-Hl]t^:l~m%7^`r#Ju` -Q"auV-ue+4kFvЫHA+W܍;nv":Y I֪jZK`D'7VW{on$R-^o}7%DҏMfP_|Ak6z Y cK_giG,ֵøsvڝ׈gY3֗q}0bf-n$3TRQuFjG"DVjh?ؔ^H"V`0Vn$S_ cڴ3ϩ[O.II户t?ɮI_B쌸 hm~D׮]~bX(~>ؗXMDzn;n$jiF)6$C}8#nq:|ͺlj)o-kO}jS-nMSP z︡ږ ZZRgLW"'9囧?bA-Ѻk/WGg{ODagywD~_ܗFd̰ RYqDHT=l5 P~u<YqCQT*CQ불bCkT ~ZUA-- XL+)U O=ǦOJTEJt#Zj7PӇRR#qJږ>d2n#ZviuxRb׈%F"$QV?#*ȧVӎyZZڲznwGڲu5E0,M9}ՄM*D%<꫗U^TP UGԧ"OmEW*FX}|h5ϩibJՁzt?#Kys\OC2 @ ̖CS5^ < pu%FRH4 wZ,*Q}%dG12t,|-vWĐ$Fs &DﳕE:! pu%FFE%]8,2ý G\bw,_]#31$Ѥ<! O{8i8lo܉c|o ,-!_֦ ЍD7^ @ @ $(1@ @@7HxQ:6̿;x@ uvsP@@(=@ @(DadǑ6QO_ﭤ<5RkQ@@s)}!prޅOI@&J4JiNo:eyTxXp{e}}M)nc3J=i#41kׯ~XWv/ٶqﲡߢN[;%n>ԓg'F:oXV 絢>_}FB,}촆R_o.>,ÍDOg7n?ZWVKMïW3~F]^[VWv/Wv7XKM?~ӳa҃0_bYLា9.Ϝߝ_'K~qXbħt6ĪZu#J.U>mU 6+&?]F]hժ}M8ڲ.`֘+¾O|t^}4#~~|)/Ֆޟb^[V+[LQEӃX<{Ǎ'RrHܤGÆziUmԪwNœRZ4qxiDDdSGRT S8}, Y*8nG_|rq&Xkbf-Df|tVOaNdHt Pj~D+ciFG4Gz%Fե~z/o-@L S~7}̓0ZVjWP.oJ(=--yV,J>nGV]K}R^aAWzDGi?)|-*d1<{vgRprw>@SGӲ?Ôi(MmFS1/,ao1b_ timS^,|L9_#7Hε[Ƶ՚G]-8[;n8􈈍&5eM za.oa ѵ9SPs7|,5|;n" X7 % t5}ơd<SXmB{DǫVgbj-ZMT354FeZ}R$|w-u9.]}X[=WG~D$)i6 k&Pʸ'wIO#=dU]M@hZ!OL^;nXW:[okw󸭇7hs#чD;m1WB4 ׌0Y jlx%JXqmbaj $I87q}xg|IJxZ[a9.9ZݰVl-߿@a }t,;9ӫXMDzILN_|hFBB^aYENzw?ےMrs8,QvߍtN6Y2}c`0psHԎ$ w꾒:*E@730'j5ykfºy6{Qu=HHzdnaǞOVʃm6h?kCokֶԫ]Zk֦G@yקV+agk08nŲo' 5N 6[yU!Gl)n$qn$t4UBQQ^RX璖'͵$dgn(dqCȎj Ok+ sIKGm~}D5Cɕi >x%aYvkHt ĺ,p#1$!}X"J-j>l5W}.j`;}jyb/fM˵eՓ[%ҁc^KB-'aUeٔGwk0,V-zDF.,׶xEv`u︡ Ջ3ʾ8Ngrro]EщCXCL KtIvF~۷Z,-Վ&MUeTz;in̡ts9 ӎZZB ֎P.jK#<+Ae@X Xꏬ7Mz-74 U =B]6+!G$!7a%qC s#u`CMlTDwaRU|cVm59SEښ5ZPv4$jpRUV/ M]JD BaԖ&Y~UkPKoH{K6|vh:ӫk ^jMgbg]V{rdqlׁztGz=t91k%<^obMzA3zCt&z;T:[GQ F퀩!~Uk~TMGZRmY]zD GZw u!;3Bsi.xY]:"҃zUfV4U-k[m(%#Tz- juD2kU ӧʔZRmWc kTJ$2.3ZlU4]+e+[Qn$RI @ haKKpDãk6{g`u`M#0ɋ@(@,)^w%0ĩ_Olz]By@D|-vBk߃-QSup}GV#3VkG(@`&װ@%u}*(YjPv2wxA 'J){~V7HNvxk@P=3YQ# [͎*&\&HMk945:NQGd-@@CI""LLAȋVdEѽ#H?Q&j&I ֟.3hjŚD'Dnc%DI}047$r/x@wmm1˲JxZNNˁ.55]O#銉oa:Uw_ƐV&0+ %mq #'Δ,p#1h$1XS_Go驗;|G`꽜i<}YYw.u7b 3V1+@ 0w AjO. w#aM#IDRax"FGE:LZs# Zf1O6qs1a]huaꠣdoaj@T ܶr칍퐁J!yK`udzVط7F(&Xan$$JgrJ ?"9hH@Vh:(Տ8T Gt`C[9,R6Z^J?]ir VGi`@%^k9-'ZUZF E ֍X'>E`W?x|W+x@@MqG"$1hə@)zĻwVeQ7@ @tHG#`F D=탑/;9^d꟝-R"$ FĬ ĬO!@ Q IG% zp9 4@J%+ݔRv-uw$r ~%&0!@ @`̑zN!@~_(H@@I"DLN}#n9 ϕ @Ƀ-{{غqbFks2wxSe`2℗ Ww= P $1eȇzD>L'^k.0@wE`mC@  'mFG{nztU#o{ @ 2H ID7+#6>C#D]|_T+š ũ-$!"@F>H IY< Pթ3&j 7MpLpժ)O%@♓[r"5*A t#$ @:?<"@d %:.Mk&^@&!P/PBS!0 $ I@.Le:h#X9VWµ \xn2;& C7I{s"0 $9 j n9p > @uU*GւWp֒H\j؇ LLn$$7t I@3D݂wJ )=R 0$Bo$$$_h0@ Nuw>zD@N৿x2M N@7H3C%wCWזׯY۟@`ncGm_@ 'u̙07Hsc/yԗV@B &vZGdE`eb07Yg Pn$$ O@'dsh@M#(J;kRt*r jyBDKEVk+ {#$1I 0>\i~%ЪT1WOO=W͙TW[hsXش6)r  Н|n$$j̆)eݟ_^EMC @{4?-Л.~Y! @`PX^̷_?m@JH)b @ H @ ~HM@ 0,;mǰi #$ljR @@<'.ebKd)>@ @!$1PQ8|G£ @$/NO+Yj%u_!1 IA6!@ @'$1!@ y@c%A(`o?lykޛ;e3@!LI"!@ @'$Ύ @__E;,Jl;d#HGI"[,C @}\Ӹ b I:@^|kg @'G4~CNI> @ ?ۡ̋ļMoxaߧ4銍 &FIbb;I[p$aQ@ؼcq(K]e@/> @ gNnc AUH @ @=^ءE!H @ @F $1t̐]8r^ @ @jd @8_&I@`$&^:@ @ȕD/@ P&vWG~剻~74 @$9sΡ?]$Mk ?X$ @{{_[JC`$z@ @Hi6,K @#$1kZ^][C<6{eE~C)7/~‚?$n&¾Oo9򝬒:؇ @+y/i @HC@r(g/ag4Ca2ܮRr$!Xh4k @(_/{Fw 0 $a8  @:Xٽ8<:<ܲwMQ̘ČOC \`Pxԃ @`'@ȜƒnC $x,~~|y6WЛ# @H@8}lIPi @ C @$yQo9ήbQD"$ $f P}Z @>m߸xyg~T3*tٌ:LW!ЏD?~Ԇ @ @ D6*A`~/_1 @#_*{< I# @H@i brF$fl @ lݿ8: B3$Ȩ0 2Af"L/ @@$J>C @'$Q| @ @DH%F !¾Oo9`2ׯ.^]c8U@ 0:7׺[N'@ OHy @ @'$1=@ @@$ ^A @&NIb{ @@Rr>K$(DA֞t=Ɏ#y @ $$ !P!uw!@@NZY|@v$ A @@IbQ @x}њa@@88@ @%-K>HI"Ǩ @C/zxP I>kqU@ ׾3y@W-'i" @Cx^Ɛi !$1@ @ @ _T Iq @%yQ V7MT"I?fIo>,cO!@8ype/; .C $dh1CB /ti @T IL5  @@yN LIbѧ @  I!@%pՉieErĸiDH%F !@ @@(>t @`bVv/60@HB_|kg J@fs~)! $bG @ @A" @ɟO+\'$Q~ A3[ߟ$wח\Lkt Fඕcml[/ (H)X{)woO_0x4@ L4J @ @@<@@ @&$iƕ^AJU`\@  s4 L$H'"pNX @;D?* VΜ.~ԔG;듙GC @@2MdG@#s @ ̉ĜM_!@ X3"*N͎BN!П7@ 0US,NI":R B k/E:A @`6$fv_|ktG @(n;.F @A7iB.NA@ @'$Ύ @ @FG1 \?yچ @"\^qpS}צpoZ(D@ @@$ NC @`_"29H @ @` '.-u~@hs"$1hϩw_`q @&$iƕ^ pΗ\ @I `iWh&# @`:Ww= 0iH;zk_:{  LC/zx#Ƶ˫L14 LDI7 @@{e6C@WH]Q @ $"@ @ %i1&=܆@$! @@N\nٻC !P$B @ )@Br puo<59x @@#_ -$lCc @\y=?œ<$ŜJwYAj8 @ $< @$pa]1u_AJAHHE 'LǗl+` A%b: 0$X\xukL/ L@ЋJ C %$t @ @@$ r}Ut҅ѤG @ؼcq(YǑ@`&$fh ֞t# @(ޛ;e:1@ @`9u]_OI?C,@ @F-P$B @HIbQOpu:eN A @%d FY1y37pյ߾  @` X^o1Pc4Ӆ_.5( D#$ % @ 0go?d9$:#Bv?};?*P:1 @E`m"„ I"8GΛHg @ Id\ @ LtcK @ [D, @?Fx@ ̅3OsE|'[(2p8 @ pU_=H@`|H'pˁs_i~! @  I,\ @ LtbIOR W"5aC @"$1pY@ @@.$r~@ @o~LT#K @ @Y@:<8@ t6m_>Ɔch!$ @ @HMI"5aC @L৿xs*@4HE !@ k%Rd 3(D1QD$ {N" b  VX^o{{&@S'3?+JBG7^ @ I,twZ~BL+ @s"$1hW@ @@6$ @ @`N\nK[@ @Y@2,8@ ЅC/zuAY@1M@ lޱ8uc]@ D5@ 8[ߧ$e @`$ft @ @1@ @ I0t @ 0>$c @Xsx|NI@ @$FN%/D Po(O,wRqn0fBIb& @ȑɃ-{9zO@zHD \xu.݂ @@HϘ @!p])@wl9ɭ*P!$1PQ@ @ 7w˒0IIbqWQe @ p<{4B/ $L @ y@W- @(C/zxEw!83$$1àe(2 @ xgoFIbn @HH'Wܳh`ʼn@*(Du@ @@$ʍGwcjo>B}*[!@%x}%!1?O@ O^y=w!NI%%iOqǗl'}R <#r $<b>#؃ Woc|ebp91 $4 @ܸxy`J!/$FC @?;"@`PH1@ @$ F” =锻G @ LI{b/4q  @ lݿ8I( ̂, @ Y@UgYĈمC@6c۩_ | IWPXi!.߹xw dN6{e%s'q@k(l#"1B 0!'.%adB+N̨1im99@@8/$$k}McaB[;oj}hfB=탙tnB$9D>B O_ jNC @ p @  I&!P:/UKC @@?D@)zh @< Ix@llBMG!@Xٽ8̎ $xt# ̏so[96~c@8CIq@ @$FN.߹ @ y@g5 @ @`dH#!0ږlH׽kS< @r&$st-&49lA 43nٲw"t>?œW^|"'$1W- %$,ÂS @@$?~}K{{_lh $1 ҅nq5<& @v>~}n@w~T  $QDp2O_ @*d<󟛓l$!%== LSGIޜPD DIbNѦg (@ @ȇwvm| IIbHڴ i ѝ5 0H $'| V @غqb?PNshixD*u=mGW?+ @ &$@1.߹xw/ VmlmXk1 @T IL5Jߐ @E@ HD,؁T IL5  @ /FU|}LD6>1r I;yU\g/߹xw}Y8 @ȄC/zx8 D ~w~4N=@z` @oy~~|yɶE @$>(@a2 @ 0;xIJI"W{]Of T @ &6߶r,Œm^lYsjũ!Kt IL'$Co>_d\ | cA@<HXb)OvM^ @| 齃/]6st!$хe!{c%@F&Pv;?GFFYH dA-6N@f@`c,?1HE@I(# @J&>%G!DB @ @c @8Nk뷏6B Hbx1` @"$1h@Ro>E&0@ @ .$<T44YB Џ@Y;+!$"\dǑJ!@ KI9 @n\[IѴ(N:t-$" @cOS8 $3B֞t p!@ ̆lBMG!@ @9@)Jŷv|zhֻ~uZ}:Yt r%V5yw l@C @ @G5@ @@C @/q#_Zc__) BIb42{׮v.߹xw} @ @ {Hهc8|G ~~|yɶO΂& @ȕ?셹z_(DIWH`eӞӇJ!@+_ֵ!@`$fz:EG) A 08;9@9 B`j$Q@ @(DaI-`|@ @(DQg@ Iǜ2;x AI0$ @ @`j$Q@ @(DadǑ7C @`* ncT@? P<$CH @ 齃/]K8< I @&p 2kt  I t16WM|-̰@ @&$(W߇; Q> DํC/zxP8[}@u $RP^|k/]A}1@ M`eb@ /N-oCmxgoX$I@ @J!$QJs.߹xwo>Ŭ1y@"PC@/)A }!V[؁ @ -$lCcs$K0g@ ̐ >.}Ov;I @kWdR֦g0dKI" @ @`$]@ @ȖD/H!@@,Rm4$IA @ 6vM{/$ƞC @F$$1"| @ ̗|cO-Gw}y` @ ߰++8w,N-xLk  0$  @ @6$  @ @#@:MB @PlQhpy@3.x@Iํfy½77N]D&$( @ @!$C2(W ^+/'sq IDq|omZ @s"s6}$L @_lV] $*7z^BX  F~7X[4@Q ID@ @@Ƌj"@&p 25e @`$f>> @@=}@R[}$}C H l>4:\CO# @9`o(D/Ɏ#5=^7]^S, !@ $"\t`|@ 0:H=8|NI|F$ @HAUSp& 3$ȔϏsihp  @BI" F@ xmW9 ͆T~ϦtpE@e)PA SGy-&t&$ @&nq6 .C 0H5= @ȋ #B7 IL+ @88 Ia\szqɶEP *r}ڭ!@|kwY B@S%$1ү3֞ta@X$2 @+$(@H,ԛ4!4庡D$bG]8rx2 L Dȴn}՗wna[J' v f2e2X܍GI"K,A 7[ҏuoY}mk_6J'HG } ĭsz):+XtLa@EӠULU̪BF>QC'xN_|kgx}jB@ 7nY9"EUsA,>B &+OpR"۪t58VyO!c,.ĴK @e0oLFPBylYkB5! OŸ%!0 ;]hW*IW9q>ZkiR.Z+RAI@@33iZMԺmWUȬ9/ók)!XM@@ hks|pO _2H}Qwr"JPe*M;LI݋XVRMNP(jeX2F }!ۧ~u\FMH@-m|@ӌ&º8+jsW d4H/ Й-}aߧU`a6B|t ^)/ք&U4P(Yn;lѓ?osAʏE''X //D^3/uE5=Ͳ[MPk c >RF,ˉ\_LTGy%~W;-߸x@ĵUZ+vyε6m_HWܳenmݿw$g/{{etW+%˼Hn_ùH( zHI"! ʒ0% %F(Bo&fapG/TCQL Cf&zV3sf٢+._Jg@~<.-f]%F^IG7'm$ JɈDFW Nܸ׿}l:}Ct aVqH arڳs%s(x#}#zgB>:7ˠ)(poӈx0Q\}V=<G)?5% SPŔ$9r%$|N|$ JɈYK\PVolߎ}fp є=$, /ӵKtY=~Qw2½O} ?{tl{(ϖQrlXc)#jѡZU% aђV$jEH>($ڵ-Q-luEL$Ѵgbx\ J&%Oh%sIZh&A(Z%ZbH1-It2%uZֱSfHS<7uy]W6IqK>$]WKմ$o~;!I@D+IZqv ),m(H?\$$D KN$5#:0C#I8ԇZ$ eA5eI]hG6dGX^I8<.*vgDP6zkmVrvGv= `m$!L,V%B%j>{bczG! ItE IfIȧM;G>J1$xȿMZQBMP?JPoZ}7LIV ߰a|압(1#-Sh5-IԪY]%&ìm,x5)g:k>öt BݳS(4lOG['hhvdێjGҀ=jGkL&5 6U^2U֚j[A$Ih$ajR'{BY.׵J(SJrP9. yT $|b$C2@#IȖz27Hx\ KɡJXYG(BXO,%[3ǸgHU&IԨ$)=4 IJMmIM"CS[Czhj+X>Hꃖ$|ԇVZ% jVhMhhI"@ƭp(Q޴.iQ"IQI=@$V68}ŏ) S]#Wo}_Y{w~d: ϣ>1r.G=l- ; i'KIbK@GMN{*KBnR%|$ )JJP*WbRj R>[ҧ*sZ)G%IZo$t-I8R6$[% I‘ЩwnI"L}uI Vw,IQmTIIU%6,c|"RWި3N[=qG.A@gE,t$L=B*IT1$!:N%)H~D*Rʑ=$eq"J .z =SÚaN٨U%ܒ%F(0)FZP aD5JuCxf…RAͭd舒$JM͙D .b17%ćNnhI"P@S͏ԝ ԰<<%a{P* >@BRr0$L=Bʟ$lFuD@F1 IhLs 6 @U0#Tf:RU%aGʈ%IDOIBKTV"u- ;<'n͹0}3KYˏ0 5,=< 1keItZ $L1B*I)ID$abВ$L=B>U#YcU!Id:Iq͒nl$8pS'&IK$j#4Q$$F굒0,IkD$QͶ(IJB IB+zKs↞"EBI/7<6ә)4i/or˃-yhVO0šV"ֵ$}fŪ>5S$|&$` @ Sz6"F4Mp)F WbDnJDuMDhb,%l-F^0NK=US1UDž$6Xr"Z4Q/FD %F55HHyцH I D'\.Tv(i ۷*!YK[h4*'9"xO02&fg3''j#jZxvFQ&aE *2JKMT-K0㏮۔.QGAyi%69ay9DyR}lZR:)M(MIB T}L] [N괡3(+=B =qC)zO-z'p%fcyK3-֭cy6u1+VvjsyꊕZ1T1`-N[ZUvɲciz\e׶M@}Z+VTel}Iˀ0=.Zߝ-k׭o-oYu-t7j-oٴHMU~#9B ObZU=!IxX.$t$7Ee4 $/J$˚J )CJZ /% ծ&$Q3CeI$_ݒDO%B,Ih ڄ$S$(-OI $tum-IQ"3IB} $oJnaIabjEKb/ItZҺ^y=qo|G '(BIBGI"A@rW;n-3cBt an$, ”-n㯬Z2:JD辘DD%Bۯ$b)6ժM8$XJvƑ%e))i%(Jn$jD%BP$EX)I(kդ %I(%B~Ґڈ{7 %Xc?z):9Bjm*MBVg$]>t]-s-sW)4%J&Q6$]>T%( #VDvcI`6aIJ)IOBE6/M%B+ qNښ)X?%jBʘ%#:$9us$ IxX.|$ YHBZ@P$ZPYzyh=zS|պHXٴG Ir !.:QBKZ#^ְ4$K]wiB$j DPU1w}%_LG?[k{?h:BZ|2Q)$ M(IBRLL/-IS"Z2|I]н %IMbq9:"Ѱ 0{[kcV*Aw~ę_LJM2餖$iV_Z0wPu$Zǃ*$ b+IH\Q-5$\>~@`~ {+]-IM7w|e,u$>Զ*IJb!d8$&T7K8CU滏[ZRLqA',!aXF@aYn$TGLIœYiYKkQ{6&>r%wdImg|ZGdI?&XKBPY=KO$ cw?Z 6ВDd@DSVD'I¡;4R嵿Da9ӤGXIRSY2v1גڙ?!?dIx^N$uZTB,;$є њ@$4{߅ާVqZzx%DіCmJMC*IHM]sHaCmCMzDPۊ)I?xgݸo<7-7#rVy*IV+Jp/$^EByZl*$h$ږ^R#Ih -nT'pXkj%1U@*[nS M@Wi=B%ѴYڏSLBeI8rHCIMh$Z5@O v- UTM0 (IBR-;I. OIB l- $18@@uǍ$$ĈIMV"9Dh C>e- Cו:aٱ$ PUZ$#:*IE!ulo9 Js[եZR.+[WjmJeoc+M$*=4@$Q>KUI‘z2Jhi${- ՖΡp,'Jb>b rT(T5wʞ!FE\Iݐߵ[ZT%Kb!5:r.'кك%)IT"j18$Z4I4Sː+p- Ͻ?ŠbMTwĈT.Z+O*Iܞ4Ia-$lW--I8t!Ik%`顩!-IM}Nງng/eij Z $QU"tVDRLv$ষuH($ $8$#sǍZ|$*Cm% a9UVU4j% V -ptӢc2] t ,n3$`eLTNKHX)_\K]z0MY2[hT 8tjF5- +W* AkCF1 (IүM{7_z[m)?'YR^V+KA l5vE I%1iC(@7]7hu] +4?U&Aƹ$ $jkrč:B>jZxX2Ҋ?uoZGWwM8}̷MS->{jso5j.'z%KKK$Q ;n$jՇY)/I8FQU2=qV}b=% KG4UpkV.o_8$ O!I+ڈ$&hp^'UPJ;nxCFGqK3s- 4޴ k2 hGkI I3hȀ@U0TZDKx.o!Y;n hKS- w MDQ}>D$I4?x:YUS$|jg$)jt$j Z/I8 Z[i$:iVsO*mK!Lfu9&qXW2t&n vbܒ$]v $>;$ )KB R0qc!K@(=qChqVеj$mHEIiN[l*Fb8HJ%@T}J<;$m)SpɎ}$чuG $SM8v֒a$ p^Rofmtqýe$Υ$^u- 0ii- m?-9j tJ,(IgkJmYDXg[$Q:`Jj+ZJj|֒uRFӜ$݆wmꄒ$A%avm7RY^6g-7%Vx.oͺgvJK8vn-F^(Zfpmi$ bfrgC7_Cŷ'f?z;~i_<:RU|ȫ2Cp?oZ7J׫KIC AW{Wg?t-U__U|/rs9H6a4<ǘpux}X:̳|TOI@ @$&37Vɛ6xi oI iK$AX$:m!Ks=% w=:%3=֒hm)ۢuF7jJ躖< $ݳʒ7g'<ߑ$5tu%c I‡Rp$`tV7Eʦ&nȟ͹f^|k׽QS]~megMbS6^W2t})g.Ś64 /zyߴIYE=!?cNdT}=e uбaiDk.|ɶ,K9ܽ81Ja U {$)|Ƙ 07_e , S$@s-jE@[aU̝8j=8,WkYYQj+MDږCO$UܒDږC4&Iauǒ$R!AI32L;la6b=U aA𼆤.$$(I!OS[=ka>tU% %F6y_}bꤒ${Z4߽C0n5h$$,%B$ 6*IXJ*I49$3m-IxE6, Ak5 0jIր}hIBPW:Q$ ! Oo!]ሡʪ:"x᳣g)U8j;emѴ4lĿ$z?x6gJq$W%>I $u"Fl{j8'ZGYd+IH$S%̉j! !sR$7q$j#թGZʔ$fJMՔ$d[zBzOIK|͛[-նѲ -}~*I!GtUB#UDĴ+XkI9|(Q& 5("`Fc*"`jøΞJ z(%$3AC/ӺUIB)It %a=2Z}Sej% ܨ% ᚒV7%V%B2%gu]I0GFU/$V%B[$V%BW4%NsO5% `is0w *I=\ufLB>%T+$'0$!$M@D, I0='nYRR%!A\ ZKBDD"I¤90 (e%ܜNx!u&K`چ=&JW=Bea$D[vN(ZR&_'J7$25(fn$ IʂTkS?zPGZt'/ykpX%%S^8^ízIοz‘gcyz^Nzj-#Y%z*/^6"oe)z*JVPY5z.*:kEJn>  G$ETMg\5Cڽ3qGTl -*=":#t΅ږJ @ˎ *]*7ʉN?KBz☻ h5KB,wM%%XY:EBb7QB︑(Q"i#JΕkI (1@#Z"I (:KBR$$ C~˒P)rnȒВD, G7$JM@%T(3qCKʋ5#w듵$R%ч^uW.| ߯SWlXQWo=-jqguHֻ#*59BP *O]WS\u?[_WSEbR,&d@sŰ޵㵥 HPA 3\ʏЧI*B@GLy S0N(o$d۴#LÃJ4pii=B73D G$H0#sHH lȒx KbAK E@fYjmKZRϒ0% X2K”$dIXΒP# :KBY+.K”$M@JJC\Y$a4W"]G$ʒY+K”$t:{J, 1($ĂDk9k,tdIc/SHYbu @k7dIHZlB/PYV1, ` hN9ֻ,*Ʈ , k W"TO(3\c~ԾvH@-W,EߥɗMн&WgTS$tBs%ȏ{mZFsٵ&@Dw .CDrbPYJP?, 9ؔ(᳖%IYbSDZU=gD +KB+Qʒq%RdIT+KBz.Q̒P:]D,afI~[W"EDUH%Q1K:eDU$$}%, 1(afID, 1^^K◿7-%'@UR1%I U7M  *#>?&^Q GXeC 2&)Ǥ>07¦7b&V-k>7plwеt ՜TכqXɧfM{Wc~(`b sP6uWsק T~iN(F~ A X$)$!=W%ʒ$kQT$!#%$qRf˪$!m%R%IUIBH.IGD$j%I.!1$!V%,IBLuU%RH^j$[^At|:KB=)&)Ky$ `fs$ aV>xJSCfcEPKcmVZp<9W% #z$D$v%N͉M)$SEұhqGdoV,\+R WקIUg:䷩Jă$Q;eODӪ, 5XXJĕ$kJg TZIBJJ$kJүDDӒQ$%-{JS6Y|$ q'Q%$1`kñ%|z# 4IzֆTh%!=%$JMztaxI"*Qp$IP%$G4I)TD$[l$[l$$Uj(aeI ID.n↕cW3Of~׹MƟ sM%QOh3\SL@zG{&0$QeuF:IiU,IBz3Q)K"7I½CJ4IPDY=B/Tp,iKpo$!ZZ_h=% U‘%aH5=qCWnD$>2?qHgDi&7#lzNh4 $L/U&$ OUSc0I¡GMYjQ%DU"VDI"*$% $Ӹ7beI؈"I!SpH, .MY\, 1JT$$ G((X2q× Dl؛5IBxUn8&n4Y%$#F$Q$ !N$k] $W%ID\U"$Ѻg[$ܻ~:$U"7I"!"K" D:Y7Z̏PZ;nXޫe/dx^siߴUiǍV%k+:vhʧ0G\S8lqѴji wN$>(HjyK:VkWmY|QI-#97g)JvY i( K$!'J$T,u_h#CIBEQ%Z% i( (!>;&n>VDIUK)JpD!ò$r$yD, ^iF$vv[^9<{=gR]F7kZ=CTu@IGnINzש )]M(WZqW?̵-l7Z.qdYe|!{G?.EMұazɥ ô5z+>zNVpCI|h-S@֘ed7PYqx)B|,dJxHHAIQmzOD@zňD@t;TNjS$:YpWH]S^HFU]I=U q1⃨i>*=NU *_H(畏P,%!|k,=Dt=Bᚕ*]P U%#N(zDܫJq8skH3u?wG*z2HG(';O9N?%H+̜׿zE a)ʓX1EB.QBJlRHXz>eGqZDJI4 xLP z&JX4S?} n *WPdy1w#xTS&7E"5 U"Q`X7gPϊlT=zD,UbD Y,'q"ID)Ou<.|S$">KHU Yj5ES'MC`n<׶l'EB1NG$IU+ʄUa%G$JG p{6eE!$4TS$T?&J>#nU"q>돱5X#'EI Q" 00D)"a  `zv%)@-̶"lC`y\ C#6q牽څNAD0{gTtArĜ< @@D􈤉T!D038#V%R+|De)*>6RGl/u}jJP%<D=qO`G=UѮJxlղ{ʆ#Qb0宵 :p?k' G(ϋS%F#!IJ G:WbD{j&JD$$vH קO=[ V%#TT?FO`@IK @@P%HJ,ۣSSR%R~:Ub="KOD &$n NP=IHYkaJ5;qɀ`ySgUN]spȀږnOJG "`(1h*1VY U=}FN{>zYq#D'\*hZ| xխ?K? &NDXszs0t(TS$tdT% 'zr&*+Q"* '$ OPesS7D@jCDz#1O'EBÀ*ș@V97Eb02ϕAPS%.*wxOCd\kT^Dr0+"ID8)#>6t%<kq>N%ZUzWފg# *7. T@YC&J"1G>zDDnzDDav٨B(F $qm]VK@tx{b1rͅI(OH$J(W-1Ebt="iDps#;VDnzECکVKZ%׾iPX@`0cSbeWʵQq[= {bZ-\XN֪U WW=MwX_ %ۺznH"\jJGt{IUDloHyjQ%I@DQ%'JZQ ID؊z>K= @ #LX|[$SKk5'Q‡ e %0zD4Y P%q,U>4!GO֊=gm'vG1$ c! EBu^'J$z\Lk>K='#^Kk1w qS$2!_(N"<&JDLJH(!$UlS$Y_S%&GL{ %|MdB٫#ޫYUj*ˀTѪD!NƘH?҃f.MdeӞ`d- Q%0qWV%|z*9238#Z5 \ ?eac=Xw`m `x@DuTZK¡Jٙ㷭M*$.++mO O;+EB숔Щ*Ay/ ߸0 ʟ~"=B̊/X?e mQcY>ҥHeQRt{lrJGУKH?-z~\ce<$!v$ӡg<{2"odm0k_L+!*C' Ϝܪo2ChԖ/=mX)$ bJ_O]YQBADNuP-gsX&qjv!HXy8U@?v9< }jWCGׯ(5|ZG tI$ZKO%vH].,4' Ia$ibKVH`p1=|{7& %皸wOIJ|͞$DP-6%==% AT Oe$Kʒ$aS0%DzN*N%I#K)6$KHbG}$ 1"z*IHJt$wo~z~"Ix*Β0ty6!ꪕn*@u)R2M9UB WMKPk{GJ O:|P+XHZ@$ P;j6'j%9H6al:=i$1V`1iM4N(}S;t90L'JhIB:i4Q\̛ЉMU$[(=KBЪ$4$3gs%LI"!M$ĸ(afI$JF,Dz%)b\eID#$ Ih[JP+\*IByT n }J$ OP%cFIigZ7#J#$s% s]3qCi <(D܍(?ѳ$WJH$I}J(I"!m%$ľNH%bZHGHI% oZHGHZHGH+JHG% qOR$w 5 =e6WB !B$ GHa$N$:`a .&( Ё=Uېj% 3WBdӧZIiKPnE5iR1I#wܵ$4)I)J IH[ZPz4Z&*$z4$zęgrq"IADQ$=}C 4k#$#5wH"Ix d0 J$CQg>M7)I8d!$Q JVB e1 C*qG&DP+\YYK%Z=B~'Z¢T -ǭ.SHҊR%X2E!I"1o?KB~t-W% =}*U(I*F(k:KB?H(FdS7Ǖ}E;ʆhjҔ$Z5H$aiM)DU03&beIV"+U0Y z47$! GHC"Iok ={_IŸ%!c]3t/eCHM{j[DW[Wj;IpM@;Iz8LܨU.,#bJCM@; KD9%$bʃ~d>b)aX3A7$@Tt$, ½$ѪAX>7IT=S!Lf-Y'"x˱nGGX@2"P/B_ \D2ZyaJU6i]IPj$jՇN]% ;aD&E$Ѫ>v-LhUj*IT1ۺ$/@t$Ii$UpHnI] uٵw+$1F`sh!?RI]  LlȒ$4IMS6Wh$Շ~JaC$>HcI#@J Z;75Kxi@FF `4k#<$jueu&EaD\U>J$;U,ILaWJH @߭LD| P"„O[ @I"U 0# ̐z$sTP5xcXL8&":$Q=R@LM$$1ϸT#Rx㱟]H 9 @@$`@J'2M^qN) @@kM I#<̚::<8c>TL~FPIM2bZܙ[8/rdAL(i/ lB15G$Ql8M(8cV$^E籢….~`4"dv$r>C1P 8#[߼jؠG Z HI |`>M3 L4☮UI="m,CE̔X$T0M3'e @ .Nl-}z@ W$H(br1"YBP% >?ݱS2%,m|@Έs.IpO Sת^z׏w';E?C( $iL=$8bDD@Daj]<+IMm C̕!àL%8GIbt80 yL+#mµdH15$scOsNvVf$ft䈹D~NUFvI"2PAHO(=-!It1r&st U M1J#PB@ (N'BP]x].1`wE߹Ywg!$P'`-(Di &@UbA$@8Ņ,H)bs  BUb̙6i`p  DbOFdh1 p^~솑$ƎCHK|K$Qr;s<TYIֶg5 04Xw)DWbbĘiC@tFHпe.@Eap I.v16Jq+V$Qnצ9Јqtu#:VP 3$$1àeĈDU"L,,$ip+7# x?,IbJ@獾AJBX'.bĤIg $#$I|{_'ks.s* W1:#$Q\r1"x  I':Y|$m~YDr'(A@Ww|L #<"Lm:UlB 5QHZ}Ĉ"ÆYv1$NB (6ޖRHg'Hp͢0w3 p?H! BO l"It5$GLM@3K!' 0.$q:@ W\s%IOؽ,@HDq*X̖NIύx;#J@S'0$rc DZ-'ɊWV| I0Y#>S,B@*tHYg3 O(1 I@G5@`0[$|b'ȓND$8ݣa@ c\ $$(1<&Fe@mI"53YchEU"p!Id܀ 7d(#ġ ID!ns~@:IdLIrGe#܀ "Pv$bA p!J$Ŀ4]ڸo!>@@=8 I y&2m@ 30:$@ 9D DjCzʴLу$1zp ssp ]57@ @# KbD}FK p 7dI˟!@ xνBHTNE<p7ε;nu.;kq}u@`2{, Qî2!2q7&C@fgѡGL&tR%!g ̒P O;aX  \f2, G,`@ HA5&D֒7c*bەX[_-9b.9x$ )%0or &I{#7%qT9Tl[ ZM?s8#GiwyA9bֺ`| O,P$, `hdh=섍lwx@3"؂ |FVD!vcE0e="E|X,jEi_L@;0;6Q%7$}/g/ܖxv0u3q#u7@ \xEOgtI}KW(jTa@@OëcC$\zZDM|Q@P% gd360l-LqVn<C&@ $QPč3n[ہfOn3.@3%xQly['nH & A&D*R 4qCe,oi-$1z =8̜ (`!@&?sI"E'za56e'je \C#̇$,7׆?jCֺtHe.&I̚z3 @Q$I6|hq`< IJBƏ@P&<-n S"Ccͽy@_$ey!?.dAI p$P"I/n|ǤOpex\ I |$VOPztYFI;:w @ %)ڶ$M[3'!a欐of>$y[ÝV!x)N8&|I$F&!x*퍪oA$@`Th7oX!Ih%ٍCBmIbδ@@A}~D? 4Z=@3@%@ĠwP4@ Pr n(  {>$b pi @o\{#l7 =r g@IHlnmM3" i&C`$!( tGYo Ib4@ș9;oHjw:8 >yX.tgh  La'IENDB`PK !iǽppt/media/image7.pngPNG  IHDR&>sRGB pHYs  ~bIDATx^{yyLf*S\4b<1QQp9 QDz""]#KGcME&TMj&buޫ޵ݧwZCx     bw@@@@    @$Q:D@@@" @@@(AHtv   D    PD @@@$8@@@@"% 4hPDE>j{~eXGTeUO:vW]Ҏ9ձwao$޸uӢfRS0sjZȼUX+#;kfL6 7|tB&d#`[H"-  /O!~JdeݔyP\S<ѽYs9OZgӮSڮcć٠8uy=z+NF|W}BIr:0;NC?}Sİl xD>A@ϫӧݕv$k{(xP@gɅ;F6 x9q?0E]rgQ7K3:E| #q2'۠S"=z9ߢ"*qK/OtN $[?%We#/X;/R5#̝? ;n2ݚ9.R<2lQNv8Ip:G!\;|:|]v ɄOx=쇿y*DĜ s)Gj?9(s8ls>2]+]Я_7D)v@@ 3':?xARf}Kǔ4o6l'c=M{OI `f(O9= 1یe   d @$"@@@@DqX@@@2 M  @26{.쭙#ƄػSvkn<) TO=]veӍv,]X,f;?ݸuz&x, N¸X,V/f&{d#]~~p*"4X0!M"d;  GY{[R*)_itUڕ]Qyg#.0[6+Dcʟn(lI[̖c]orx 1T_)I:@ 7#Prt2~FBF ;s:9a$+:!Gs*7E@<,ri̎<&WkW~٦}EQB:U2HS'JTah1iw5"h\~p)vӜ*cg:{yP8V5;sϗ932;]C:$Z?[bXÑ NFO*[gw)U0TwwTfZ>.  @S":_'XرVn[ TWzv{vexc2ӟ(z튭:@aowy^{K! B_8T8X9Xv4 gsI .r"Mq)=Q >}0ύd#8ַWDb Ra ֎7  P@͹/1GuSз:lpNӾ]Es \*'CJ{δd9BϱL&8&D?og~83޿ N|G;#⃙3-d3$zЯzIdۓ " @?2wyp'9|Oo eGلeftm#MjN3)~c!f~3/VuڵN{jY[Y`10h"'0oD@O=ݫsgb_tIZ-z@}7t>X< lݱWz`62?c]/"+Vu{B7Xg~d+~e{Dm-3D1  @r?w"U0#L{dCs#yTmJ]{RfKRT-nu}+ :>D딻wrk[rLLE$F" @U<+0n[BJ]b"V<_V+^ +cĺd묶y ޝ_x2zRN$29 +\vo?=T\-V_UodLv5JV}ŮE~W Yjv n-^F"'B~oS71ͤ\wpD ӜG+4/_7ђ7Wf  @?O.ٹV5lp ^G1O/uvu zl$eD=ύPvWZ蒄n0%|=:H:c4T;(q.q$tgPDAЏRy0Q#SBIj:N><J+章Q" _ O߂%Fh7n4`R@@@#@++J x h[> jE~{[%*Ux3K Vk#{>   d  @@@+@$W@@@@ "     +    Dl@@@ Ic}@@@@A@3@d    j=7f5)@B{m]k>>O|8h%7"  ]7;/co.Rw辣](֤i4tH"#@@@@h#'I%${fP2@@@ bX0f^ٹx ܗn3tvG'z>?4-4ُO'mKҮ>b>J{ >X@@@@pYӹYӼkо"Ү-[udu;(-]&݈]Qu"hh%ae@@@g}C\6;:_"଀   P@ʹpߦoKl"ZIdF@@@r0z'?#R ef> LY;hӔ7K$dY@@@h#L}I>5$x$hrw Hpy    @3\WDOʆ   x^ֱ\C=F,#S mq/I.lsR4#  jmй%+K+dE[L`evZD\;?2X0]b!GʼnoDCnqگg |@@@@v>"r    @$Q:D@@@@@@@$J@o.}:uӤgD@@" N d{%ਰ @@@D>>. 1{zBO[&   P#"J54ARb JMT    @FDAvfEA0+/G@@ `¨k# #5(V S|I#   @"4zotVL]Qf &2e#   P@td}qzxbCnHL7z% o8H@@(K@B/d"~]Zyc+u?l<1fr'4o@yijw?֧.G^Ne   @\E   $8EJ0n0F @@D5@@@@$gWݹt{bܿfm   |"װg3ߨ{-(?   #@$)Qc5{'ָ@@n">{׳@@@gC92xUߛrJ^@@@  (rȤ%nZ؊!x    D    PD @@@$8 ,µ㽇@@@IpR%N#ym"  \H#   PO"z7J   @$j~s+᫣2%7+w,[X@@@t"A 0k'VkA@@@ (֛,0B   НDyܣj=@A@@@ o">   IpZT]wU/"C@@/@$ߌw"f8]@@@$J@g    @$9   %IޅM ?ŠSe@@@vDL-{x    IpFd,0czqqV@@@hD.UC@@@D=6/ۏvEp?W!  "@$Q ;;E@@@$ H\嗽eJ$H7. z +#  TVH!   d"&ݼ& %RbUzGSn#  QHG2#   P{"B*   @$xJ+-\;ݯŎ@@@  ($ݑ   T"뒛?~_)1   @""Dl @@@ I~~"k!  tDi   @$*u8jYY?QrSh@@@R$Jg   tDy   @Dw7\.%UD@@-@$7    @z"l@@@b I&kn  @DyM(H eʆ;}) -A@@YD: B[F0嵗kOpA?u=E@@ "l & #my.l0v  @$~+QY?j_s*J#{gls4asL04D(-  U ޲^qXV F|(y]y/W%6  @}${(yc"A%"JKܫ׶ݠe>fOd1݁ehZFa gIs   P_";J4%$Q1e070;JFvG{N)D@@\HO_ Zz #^߾D_~kB`Rb%F$ivZFh!ϳ&l@@DM=kG侏.A0W[џ3f9vr2~=2 Sn!yO0ٞl @@$(S* $#^p/{܌AM$FB!yٷO%'2! TXHXv?-pZyڀ)X$cCW+sWJ`[@@Dݏ 寓@>#4-#LOa Os;v)y$aGs˓'! tDT v,S2IsgaDbңdJ*QG  YHG/\;xs]:<:F0Bb0as˿/ϓ'lxk #$8ooYr#  PDص-Cj[2 :<٠ަF<{lgh0R01BÈg~Ch(   h!%$)ٱYGZFxw쒧SuIƺG\>I%|NiA@^< ,𴬠l7_\5f2SylM ?S^w^7yuV^5oӬk!{}&;]y  f jѵDFU}+e #LQd!y>I%2b   ˺]Id ye6ЂL .zP{ؒN1lRnӰ 20B˙G!  P͡ʻ]I4lai0<‰$tX}F2˙S_#}}t S0bI*G@DeM~$!%4=\j Xva أi8FD<.mԠ5-0u/@@DB5\@NZ O%[mu*嗽3U2ԅ>e~Tf6.P2S׿ʿi4S_&R6 Y_}MOS4' حda0"GFj2?L5(   m#0hРH$鑙GI,2DˆkT򈹷+h1k'41j!)7~D"IKթ,#\;طJ?[4ܬ/aל./5x|P&]5DD<ÓGF'K #dLו>Z^4^:X^YlзTٵgryHiMQG{Nqʦh᳭}V/327hcEFlSoc   @* ;GrH[ICHn%aHJGnaL]^:[+6 {>C{%Ч.0#2M0hHBU&i">#FH џM\esF>_P  $"&V]<\Ǻ$d;:µeй[A@e:k']f~Ltʍis\@O0٦O #8"ܕfm~:e~GTlm0T;D@D}[rqm3§i@HBS+&Knߛ0i%aK%b5!iwFHNǮ"3#?<A%>48>x1^C#wb&ytL[kFߦCiCFЙ4LYAÈ%0eR3ƎI }g[/2]yhS'ϟ~)!0b  4XH!':}ӽlVHNgL5%ҤU#_#v{rÈ5pXdoˆ? h2@@$@$I?#*G܎*J:N$A*A-#!{T;2B}GF0 %K@@R I›C;О%߼H6H T"F8]KG![hw",0F8i0# T}FȎ2ܸA$Q/Uʀ  PDaԹNё%J550s38ht0>p΃oxkNÖ;#mHFc6RE<",ۅa< 3B!BsP"M6  P"qـ=e2D-DTJJtm!Vڇem[FDc6q/2r8RhC y!)c6-ӝoozGW9f-#*FyE" sA@hD]k*ӬF܈$Lu#8( aDNa$z<0|$M@@$*ο}]TK" ;pTcΉmf\Oݑ=MC~jwp%fhOϵe #wp ]ϡ}F6e #<|4 - ja/Z*Z"}MS\@@o" o G =DV$dzF7nl%ጩ$8sۅfaK=`3nzȒ?yde/<<a;b􆷵6I6}b}HОc210)D})  @.D`$ "DhAۥ9NџJ]c̏<#V:Jv z}~g7752MÔ6h!G;'T"/U6  PD1^L! $d-Cddk2]vޚ{ZUs*Y?2[ʣ"$BvCtl.aBZFF,}Fhyhc6S0BJnhVMF0gD$'QW#?h# 4FH62H¾ضM#٣w5ziE>BπQT;ʘ>0nP͖D  I$IA%޼׹NIH i+aU_$_Ḏ}IuF<žqqD$A2;# G4/0Cip}I+Ru)  @Dy g}g _}k c=F +MygWuui_i{]/ԁwnӐZ.HRķiHSR ;A@Dms㆔;"GFNKVs; EvI =cJMyatǰ}`N#J0BO9"|YSP@@o" oW .T¾2VnP?*- f7-PqAkFԛ>*;L/+/Offhu; > R8l$H+h 0Bi8fN;B;uڵ0"!u(k4 u-#Fwiu'@@!@$QܸzMu$GcJByC{01D9k ?Ag/项sV8:w>Z5KU7[yyo;2B֑;54{44vM9]aܹ8}Lr:Yg]xIS/ϡ=u M#mF2O$a# 4CH61xFʢ#n$wmC57nح$G0B.wj$Mya4_1tфީѰ4B" |S@@$7iA@eG=D K}cYm)-#n&̀fJ9c~WXOut?G0mvan"aaDk}]Klկn6,e#aMu̯Fh%aNf@@!@$Qqôk8gt%mNz#3g޵r-,2DK_\vޒKB$n%>Ht6aiZCXSò]ӼoӨ`az&[9|:[aD  DTenvc׌MdIH@eD# i"vqJ004X hBmSc8p/A#h #Rq-#aĈUuԈ;^{Q(k# d,@$1h~ %kcs8$<<[IȚE/"1P6#2'plFe1h\C" ׉2Bޮ{nRԹDОXiݣD:C$a!  @}$jsBLpVc ]⑄2n L #[g}ӜvJF s 0 #4"5E@ sZ# ,ux%QVY;՜X-##f&C[Fai3FHGȔH̚   aj w"=}1о-塃L7n.Ԑ; bH!UAvMRkf25pX:ØMlOY!t4 sch-#L 1kn4eFCF*Uj   aj3$k.X^Gk$a/YȈGFlsl3E__ }KZ nhϔaDk}I4>8_J}JJ$Ql   !@$TUHbSsM%d{,-ѣl!߬)Otݦ!fDFީ&.Hf\OMCO'i:,D/1ޅ  PM"jRk%qn Qh>,vneqk{/lVhRˈ*&$m RYXaE3{}d"d @@D<.! F{"5^ qAzŶsG800"#׾gW͏Fi_Lw_e])oϙ Gw`)+[F#Z?3Z.l|$H5@@#@$Qcѡ$I_.ye}#}~+L vMڵKA1#"N_ˆ}|l܄C #d2Fh #L0Qd#cԺיEgFySghM7F2B##d:l %&  @$K805t"4 [!B3 -Ɉ,o)L[6dvi覢;il!?[F8z{FH!C3J#cBYn(7:j!3Dʦ  DHX%"?Dn!elW<]'8Nc`>~G^\fm0bT0Œa"0B=#$|>  P#"4[:yD[*:N=&#H”տ7K2Bopn0 vaTa|_G.tk>ЗY贌љY'yEHڷ!"D}<4nzdG#L2Ba7<.'F+ =R<@@!@$Qh#On39lwiW췴 &È1G ;OӐaپ-BdFc~>#dmP^l|H"@@,@$QM" Rl1LB'bU `iyeY-Ў$[FFGM%|ˆֵ?md\k#q[ߗihB5 WS[FHN[!5eE@ @*DD$YX;/|F^G VNDwgR' Ѓb7O26Vea)L+imFTIx~  @]$rZ $& _-?`MsYxV2tpPf; m UM"LP.gQ[F87JFH 3 #N]%#$:~o  @$q-VIK%:V/Y0+.C by%3Fl^wIwmFTaً viV$3XY"yL$:~  @$~ a:h%aMZLk.998QMQEVQB^<7h7K!?r" ]0BF:܆>F t8"_S>@@D1 \sO$ K r 1 G}imAh]GyF ꩫuMCƒ$u*)3RǞ4VFt`iߕ`4:ΨAFF($$ОRMCh #d*yL$9   @3$x=StPB# yD??~?ǭg|xn]b3Y5ʚVݑD040BoSZLJF{&j.)h#L5Ǩu3Ό\Udz̯^י+_օgK>tŷezֲtO陽ўuᰥd2=e5dF4osL PI   ֡FH$<# η\ںZǼ;/آEd z0eA$8/xdٽuHݦ<[FF2F nYXˈlg(S#FH#|y   @$rZ`$!M$$І &"$P;ʆRfM%Z3[o}q]^Xi4ЃzM#k-/C[FfB]ZF IT{  d%@$d$#]$4HBW &ф4X5/2yzd+!dyϞMo7W$4/ {;(6No>0E$1wHI6'Y/2%t  @$<^)È-|}DBg*a6h w?/YKIdF:!aIح$ #"qA2i m-#yhShL5"age@@ZIs #5_mFjbVHb״ .~{YWg0"A4yk$aHӗ>q4"Fo5HF JF 8~W͔JG}s  @$>`9v5̍>4I郉?s7\ʒ)7OSYbϛrDB{>AgzJ‰$te'0uw_eyi`dibiphO]^0B!=8hzÅ<{.(xV[FH/֨6 F7㶗!?k# 4FHC2p jZFcʆ+:ڽ}ˆ# #_L~DqɄ+$dƤ2#/.u`!I,#$vM9\# ,r # ,4L!%tlS)ی#tDm@@$@6pJlˆc-y\" #$$GM㉕7{aR#$S IO504к7 q4]鎣iHe #n~w.N532=_c=b:wQ2=w2}`Ѻ[)ӇLZLM!G>`LOq>q0A>pLZ2RCzq2=iޖn 3a~#  @$=7cKkf;\cG '0yĪg-D@.1IB;# 4-#f$445ޑa?jFHM>2X:wjH4tYyD]D" WH"cWp }νv$!yFh*aU'o$BsL9#ˆq}gu*GѮ)}kF1itˆw-HaZF:Rl򈼾.  @$?vYM5K[}IyDN%$XwkZDz[zGej܎$4v$!/ݗDҖFdFE.ⶌ{ˁ$2 #ZL5 u7}F-LF6M$axFHGR~3k_z"vY# 4@H"˃X0T줹w?N=5о$qYA[IKD0M #LU.+pG:GdͶ@@'@$1fa+y CkN'8#%̼K٦aXC4Xj0X|ܖ%  TUH"#S;>Xv$dMPn%aYA# qR 'aT<"og6  Pm"ǧ.yw/Q`ǖ᥹MTDA;# G:v$aYnGv*a2k[ nHY{Wަav$At]?"h!dglJoxHvɐ~hO`1-#_P@@1De>yԏϗEhpwpB{o0$>`LO'fV+e)y}G@hDی<#N Y5u^u|5ӫe\_ժFH[~gdܼ3(%A2,#ͻ$0#e>OEZFSב0BҜf 2'K.-#KMCl7 e4 3/iVq3nәclov/Cc}2  uhO X!"uygU>SwT0¾8׺FF@J~!"b5@@# Y#ڦMcW 55x'i^|hq2=g+ˏ3;^{/뜷:so^Mk~7L4TE|6RVoIзh3\ovE){ŐzݦF#8giwj BwO9p:S|N"^/Y@@fIx.#FQwpFӨBhDACD@AJ  !@$AۢZF(0É 0B1[Dϝ}h$2ÖN_bFӈMNX\  0"ڵyA)6apŭ{宩_2%7]Wr,,Ko!evѧ/bpg"&q1k6ީф}kfެWJe=_T60hNwO]Yˆ:   h{#F" #$ aā,)3va4\|4Z2Rfd4{BM[&輌mSeFFxqL4^Z3Gfd4W6\-#.@@ IS%,kFU3nmh`d`(}H1iфH2`ujbw*븞;&z 5zM]3@ U# ! ]H= G(>бKq60inӈuGDk"nӨ]!vJb?hQ_Z@@ID&aDęyD=Cٲv#iT'pZ(ء@FpzЃn3aˎ$#  4CH8G|UW{`2qk2{`]xު_z7ЅY@_,P[;%=h6e h!'9%}F(@@*"@$:D:ԥ>MhNac2" ӊ2ˆ`|l|[wN>\7/wM9B^_zd^Î$t Xڧ(  (@$AӖ-#u -#rm^IaZFFT6  % tu$A4g]B;&&qt4 ~-#zn]g{Fpxi-#8"څrmD=|y/   to$QVq/!,lSBNygDe_lIaXm oaޣ!ie;F)7d'{#B@@4(%hFa`3 #d~I2z:k2?dsM:Q0"Kˆ,|0}F8wǐ0 R}@@1;8vڭ/>aI6?wO]-#tSz=;^p5 # ΍ 㶾Z8Ёe-#FiM:ަw,0_2 @@.5t`F"  N"̛HFYٵgU!2}X:XVeFOiPbO?vaGCʟ6 e9+^yc HL@@(-DyDaai=ugu g>uɸЧ]2ն~jci?˳kެ&aKO lf{wfΌ)smմh(mOYF0j#b*;fԗ7%OQ/OSB+ӔFn1j2*lA4{BMCo})FǗJ  TD#bt8ID+}ǃ3 #ӵ0bؒ'I$Q=@@p.{Ksqܮ uF}Èɛ3#tt][$m"ٿdYDapa ޑ!mMD61,¤& w  UhZ+`$A!&߻1t!2%X&h{@8Fa3,FpߖbbaiѡeDaA-CKYgy̩eً}&8BT   ݹ!Nq"vDR{p #9+^Y0B".mG@@*(H":8Ჵz}R4~2\4^/6 eOցn$})[FFvD D@@\iDˆcĹkjy>L neӤ`;`fڳ[?maܹMô0B".mG@@*(МH\_v`Y #vbIXHa{OdXF,w`F~ 3KEQwN%j23zD D@@\QDÈ.-OAz2AŇv4aieDaD64NaVc4u}FGIto;  PAFEra٤M #lzu)7}nkk{*F7 Mi_kˆC9i M#AAQ_? @@DB 5{܅\szin8v }iQFha:DeÈ^Aw =gyWNaĩ?ODC@@͉$B(eI6OO_Ґv}// #տ\<[4}Fؗv!#- 6Q4K}4-!NH  t@CL$ c4m1j<:ȵo3EvasF)=F0  }$aw`IaerFح$ T,eIwF@*GDFyCRSӾHa)m>##zFn0at:E@@^{ݧ)-#2R4}.2:ġΉ'B#MC|ZF81:}:s-/:7Ba߾z6  ](EaD-ˆ`C >*DVBPb!>,MY2l-thO3v` #ta~Se@@j$P?ٝȍx>8gn_=0NwcԗÖ_e?Ӂkhϼ[FXağeF #l<𡡏,-+2 #KQC@@kjIFn#}Fhlw\0¹#4NyOsj0 DߦQ0Bе_T@@vˆn#4k(8B{&XFDQ08.}su  t@m" ˆaD`2„&~a;Ki ަ1Dߺp1M$WG@@ " ˆ. #䠏A-#f d{;bψV.R,M Ѱ:"  t@# ˆO/whOs@N #40DFuMS֦Y4(28:$%]&i͞<kG;m6ۜZ#  P;F9IT4osL2q=dL o92q=v2q=_0T:=ehO><}]qᳺDZ>#Svѧubۅ2[[j~ҙxׁwt;@1Z>[$崋dDj40ьca i-7y踞;'}5?+qaĮ)G 긞/LؙfV=ӿ*/Y}|&].i?fw[d^ˆ/?,sϜxΚK9 x|ioxkZ1],#4d^~CN?=G:_G7d   (PZ$w!vaqDǖ2BԆȩe%J , #n[&Mˈ{n<"U  @G&E s@#n lab4#FH#=@@I H# #n#ldz /L #*FZﻧ;&~Nػ[|5Ph&-#pG"  P@GT!8Y}d0"0tEŦVcR+>h%hطiB #df7. #d͝}FhWo#W~>cC;6ݵMo#J?@@$LQ0BZFh}hAhC}<äya0B*NW?A@@ GGOd^7㶗їZ⪙2s6UfN[&Oۍ!?2X}F<灖m=i(H-Dѷi:98oo Μ׭)wl4 ZF8O< @@@.ӅDÈoCW|3$}tUpG.8 GtԺӾ3zӁ#Zݦ![val IaD;#  Pk|#va4RZFHwM؟GF(he3.S$vNBkfGQo“G4R/@@$~MJaį`s2bۅRb+e_K7}Xj1e #Fz}Vˆn"膣L@@Y Hjaޯ[s 5О|^t^Y灷҇:LFH;b#]MCSaDeC@@ +# DArw/m=\-ҏ8pTaD뜱 #Yeg7+1FV_m0"?[  @r$FHR؁콛[FѱQ/R#vM9Bh˭%w>"䑸e0"/ˆ\y8  %zVψm ˁFC;Mk޸4d!a 2#s #̸C{ +FQH+F8oqu?yYu 0yǔ!  /K$!?qo*3'-@Cz?;NfN]GO-)=('#ox\O\3LtL_0Tg>+G{NY^Ot\OyОi@~GiFaNH$Q@gzΊWd {/xǚ2ggae@@&}$Q0BGh$kd⡵d;z*y1s:47h^Ѫ/σ7%XFI@*>##eDl@@: dIcmHh!4asi&h]F#b5@@D Hܩm!Cc<4a,1oQzG1}d;-##Cm"  Pw# `WC2o+NdMRaFNFFўS;Ɵ?vw[e^ OkgĐw>}z}FxQZ@@% dҁl'}ˈasm(6 2„zP$31ou}|Mg[7Z/Wö;^K`Y}X}k, #N}O|-!a:@@@& dI4&ׁ厾#AQ0Bb#^5"hҗ,uA@@vGGS Zz ##~K]{lVvZI{ɶ [/,FzWnokɖ?UZyh]{R/Oii:ܕ6?X-#d}s| 2   #q$![oF;+|syO!7Yr̯tĪ #.!$n!5z''KP0Z@@@@Ƒݨi;PPI%eDsr7aDo    Q HBI9@@F dIo]Ǹ /uGtwhN #8\ɗg~Z`:ڥ<\-#d̓#t#&蛯ОXM#זRz8#θǮL-yRA  t@5;0#Dt!?v'ueߒVSt#%z7n/Zm%hԒu:#->"t4J8yg]dMe@@r" q,ψRˆ᠛2K:ަ0Bh27<5d4a:C{F2ߊ5@@@ [" # 0B.7nHS}00B5xceyquf,}ןk>辧~|rΧo|8i[C@@ dIKh|ﬣdho88kh7{49KmJyˎɴXJJi| #hoŚ   @fDw7 j\o~4xx2~/dȕ}/49EimqJ϶gzh߷[F@@ dI<0[3bjDu|U ej߉#/uD3WeX`ID_9#sŦ_:IvM :   @6iРA]FA:{F} #eam~i5@@J H{ZF0BL<-#ԅ{ ' Hp@@(K HB X:800BvS2l\G34b   Pw,# #u\UZrЗ~IglhZBMCa]iYH!&ŷca);q=eS:'acK@@@ [,# -YÈ2OkORaDk>?-`hO'q N+C撚G3h#]O\sL4dɾϕ)aD,l @@(q$ul &n-҅'|VG x* Ö<)/O1Fx   P@fZТ6 )p0#ޗ/}au8gM-&(ƙ   Y dIh!KۈJmaZFEa~$ہ    dIhqMFh!n(e(&85/1#׏e_5   @Dz0$ (+#30"  @Ƒ3g4 _a,a/S,Y?l]Fc$10Mcej(d^:u224L_gog/j-2"#  @$Z @F]0?q0'u9uឧ4O'-d>#}$xC}@Y  #qKk&(!  @GˆE{_{sw&0m d-#Z#LQ #g0v  %Lcn# ܦ>i8a&mQ/y-#|^@@@`#.#N; Ѱ0|fN}2ԏϗ鐫wmNew['̉szܦQC@@n8JNBGd~lsۅ}o`_̭XX5p2œFIT˦/yaD7Qw@@@.*dgD-ˆ1ߖr|LiQ'S@@@ @.D-#0b/I$!SY C{6e9 #}y   P@T f4 NnӈFljQ >mj    IT?}F5Miuұ-#0B}i"3#׼KΜt澙_Utﬣt+_ՙf-ӳoLۺe/'geh2tL_0TWh!?}jH]mH{I#Y>#F|[~y׼৚ j   PA|#<Mxbm}UZgDDaKY6 ˆOgˆ ~Q$@@Z#*3.ayso)iF##|>y/   `|# N.M;c6Gwe:z宩_ҙX-#d} #dyDDqֲt?Ag>+G{N鰥GZ⪙Mv@w#˼SxZFdu   P@D2m,cc԰oӐN%[)ӇL #8M#U  *{$!FmyReM"#r=wu @@@ "t`Ydy-#8kmҁe@@\HB l(! M0[_=tű26OY{q@@@ W".c9n˟ %;&~Nc6/wNΔF<,s=o]W,jjSl@@@((JC I%#LQ@!@ #4i# "  IHߓcˈR ZFs^hQ6B@@(.5hE.T+VcE֙q[י}6 YY(e40Gh}:2=e5dzڢesdz=2}jH.ӧ{y[e̜8gMM?kfʌo.myo^X9-g'8jկ1ƹM#1oD@@IhF̣=hM5(2|{}SAB7"  (4J5hd;<"}![FtIߴge@@@:DG46vaTSHvaD0:#@@H Pt$!E #R2~3""8fͯll NzH[@@@J$4КԺKi.3Gw`I)no0"W^6  (-hp!z0|F;8> d/u2yqeˆD  ,PN$!lݾO'|Vn~Wcd:f;F=]qLZLО-#dM #.zp_О<"zhOY]ܦF4#2O4#H{@@@J$JJ$#~INuvOLGyC_qΜ727;^K+_Յ>Zg*ЭcHv>;N'-d9eJa2!Ȝ "   Im!~݅daDgjMFbe@@@2# siԈM.aS(eپqu//m]rKw^zqF)Kv  t@ɑI%zƷufe:z1ihȣ4e0 -#ʒg   @2# )w[})?\ 3'78vڭ/> Cʀ  t@" M%FӾ"ӑkЗ{f)SʆL:.I] E@@@Պ$4P/6 0B{4-#LIuU> #|t(   LaK= #}y   P}EBfJ6 2+^^{yl ##8i*D@@&PHBSRn(2 #W6`ZF$8 #@@@Ս$4 Hv*tS6Kly   @*Ih*qWG[_֗]qLZL.<>sL-' }dkϐE[|.2Q2=g3eye2 7`{eLs_,3'^'SˆĪ@@@U$DvF G(0''aD5?* @@@ [Dv*ז y6(eDh_ `ٞl @@@$N/;5[FT3Oqҝ̱n}y2s̔_pI7MWU<3E*ʆ  )PH¤2Ӥ09ޡD"? @@@" M%$v`Wt`)Mh4Q$@@@._$Le@ MaasFd<_ *xP(   PMZFJ)#gLYC5? @@@# M%~eZ4@ y    @JzGZysq^=Sގ   @Iة̟ҳ2sҼ2}2=qo*3/2s/\fW6Pfpë[ui_@@@0 $>\BNq@@@*.ШH:Cˈ|@@@I86n>˩;   @Ik%}FTH   ]+H"M:t`@@@H1M'Ld!   FK#4d@@@ `     @\"b   Id&@@@@ D\1G@@@ $2@d    WH"#   @D @@@+@$W@@@@ "     +    g261hРַFe(O@@@w$RGsW@@@b 4 򈘇@@@(S! #<7   J"~y   &ЄH&>@@@$#zއ   @$#/%ppt/media/image6.pngPNG  IHDR&>sRGB pHYs  ~nIDATx^?9V6d^Gבh Wu@VBye5`/*m#ٳ{ Xzsx>A2HO"Q8I#>򻿼~aŹcͫ!d#YL>^_mH ƽ|ü'X/`y1\R>#Y8W_ndEoEStwn?ӖSx? @[pzoA͞-J!`N\0E`'Gb`}L" PЇ@@=J Lԓl13pO$m"Lt#B m_x!S%81XFKt]bD=C,,I$[&|VA#8)@`y˧!Jد9Á$„"0[euHUbƬ3 9T a658fLD,LluDnpF#!PI`$ᯡJpjEUZ#eO((ȱzg4 PFZx ,(,&ά.F UTd8UTdq32&utw!Hfݍ Y̽w>Hp߭y2[h5$Uۙ?<y#1o;f_}{XgKYXqgN_o8#ũOԗ$w;jN[@0Id_kG;hS2ي"=|Lnc\u@?KH8`7>$El ts.o0&Ht8#a|'-w_ bk4/zg*wڇW mۼBe֒sn"I{kpiAzד! jp&r'bDOGo}-y}7qv#嬻Nvºqbt FJlo9$"˿lߺV\\]~&o%}޸" u%X _?>7ߵmbC:IP\b<)+t1ԯx@~}C=7?|S3{Kԉ_pṮ9s4QCwӔYp@>淿|dߐ_=|YBs8z!֍_t$ԊD.ΚKZ9h+6HKTBVx ĝg}1k; ‡8ab$~iFx|﷟p`lR8;Xu[*D eH$V/Rہ!Ӂ,}V7wMxa/b/\iqA2+F87/$*n_lK;|^-^ȏas wMxa(S RĞ9̾9xSn|8PaO~h\9rQĹRq3E[oC|$w=8_`y^1/wSqw!!'n}:'NK) 8I|+Iͣ ^Ƴp%1$\ #C(#k>~lݷ$$"8MĖ Srò{݇p_5CP+~/|ރC7(y{cA؊_-\U4U!_zDnesX.+ݥӁ:үD C]r`Lqxw^AcS|HGuad@m>Jc9WB)Y[^),wp8Ν; stv:0>zأCJ+kj)wBI`C;i"Q>P")sNIjm48JA J/tNo;E9! G׍Np$_q~^0JSw&M}2yş-o #LCHSM GV 57 |FDžX;BeSÁC/KnV8n~>߽܍pʠJOO[Z % 0c5?ǃÝ5Fzu,xrſܾ{I6RHq֬MKXџĮ+ Ɗ)Zux\Q2Q@@(OCHG} ēĘ!jϝT^5>ES6lX2;N#1?|x#׈R,>qAy,aKY@· ʌ'Cc/S8+7Y6ᎥC^Y{0/ % Ê%ºk,$.Ÿ454R uo&m |,; IH΀_b,-kXAK HܑaKd;)B5WCjHlWmrJ}V,%EeavKK.V|X>7ǴґԌDPY*ur9ErC8MQxֹ^vn%NUrTHXP={+r~9>-*AwxGI( I[M!:y^I$[x>B fI^\> 㭙Z,tJ<^7IcHh g%{k_(7 ]·s4PI?88(hXx;e}#*~ =sf`6K0pHM9^nLK[ U鏟Nsuj+*?O1CD>@(^Hgb AaV?($6=H a/t${-"}цMr:]<[ nDWӲYEB$`s&&#F\IH,U nDԪƢ$nܼ c6,ބqV O.(ſFB@pπe**6vChyhl `(݋(6Dkg.q-Bx$2uLκ)">ITSqrдԿJS| ?P'V;n%Qp.o$.$;h2<"~|9V?|n>o;߼wcg*?rkdupn8â|#>DOO9*gb#nF#o.OINixGrYCPYEHq}QYEňG4ãMI¹?)bNS8z2as7er?H `9Lk:xQIq'~dC^ݙx], {K%~rb['jTesVRϯֻWB?ݰ!Eq -imP%sC4x=0Ơɫ㹪ĥ/&PK6J]r3%X]R#ӴCL;6ʒ;![4XG<gVy#qy|M{M2#!z:4껀_M] nF: _ <$&v翛{7n÷uïwvyP_%{gb]Cm߁DsfQ o?4_D~@c|3އ0yٓ0?F k=b .!6~^`e95!䄅:9|B.u$i5y.+4sȻ[IJꙿs;yV5M Ż$.CA·6W?;'aw)q ONB.<ߴ`|#V)O? 5$ /Z_?x=*Ww@KBNYu9|=̍m1f?tFC8`Ϻ\:/O- _:P,vHct5PPr;'( / ;$Q-Im~$.$_(I,ܹy=,7p=H B(IjH@VΣ!.2D'_*g.$ԟt; B$qXlcŔ0Cg_7]wĈZ.xR)8Kr|ڏ;ΜI9|@s+2L$_SFK\%ywD$qp^ߍ9(Ų[GI YDĄy-]G#aSH$%.Kz< ^*io5~Wi 6{~<ryxupK?UF qV|⮠-|=-s<^ !A?4e\Nc|uHEMҎu+uz(.C8 xCel[KҞ>c#*+ o~3x|VO?lf~YQ{-D..+&Ko-[ltcl42n8+xB#o̦eX$zJo$˶ǔ%Gg>8wWKqwpOϿe>|˟]_,pJsVXp+rDբķI"r bi[x_ ޭ,I1 -#7*6|OJk-˨j:U,~ViΎ$~>6?[x+]§}IsB$1a<+!Fy,#ϪAnPwf+g^[%Plf+r{ç↼ҷvMÔ_o~e3MG:)Cu%FY3kzӨ?+! GÖ+/ObVԏ;XBpnHKMq% Qpc]]:lcn>ټ2~sG=zwL\?^~n?gmk_n/>wLԗtgcT [m!CX!GxqwY{B7-V|ub-]'/eU?E/R ƏpzA}DZ/瞀K2oo$å i9@Z0Λ7,Kf=HH1@e.BDpt]<p>|7#NBwC,ne٬%p??%4Lepp쇽pK~!4R@Qvkjڧ-N;)DSvp| *A +# tf wLF.秄o5j4h_h$F8}!Hĉ1QH>65H\_HzϚeEq |~I= _Bn8BawX:MS8ˍva/ Ȭd dҺ[k~ ~88oȱKAr]}p wEE% \BX-;z֏ .5 -W&K׸]6aD@8!o/۽*u_ ?%F:`lD;ˏ:,z ]'& 8?3nدT 8n|Ptgd֯/r3S8MQx-W\ɟsCN[ǽM/gr{ NzVn:| F)lO#F,%M0~PX I^>~SJ䷀dC:mKr*Pd,o$\B p|B!~GI %Β<,4\> TiX̥㯪;?kEzwqʢ{[01O"1-g;k"-1Υ(&,TD~,GqB?$N>\*o&75^ õc'Z/Hg A?ʡo$.I<@b}Ӝc#r鑇-G;u \:X70[oy)1t8ؓU7oYO$?ON%⨽T䲢CNA~On9M,>5:RǟHN g/53~?  -+ F d ʌϒ$KbvBe*׈¡/#E)W."sw-돪AVV c# OWkrkj@xcR|D`GtC}g4|3@O[ McnoeF/-6 !0o^GeΤdMb1t:˳0U.{61R/uJ_.zGZVd̲L=1;n?US6>bO)@cϒm \(av`8Lp|bd"7{%w8D)IxA% @(yV )qP6񊱑 Z"4o%27f%N┩Za@ZX{ö91+Mr;nV"on3ZDZ[/ΜٍعmqpånFӇ]dMMN@3u׽'S$' b]!%Nkl$~!ìow9rlڊPfl)o"}uLp(r ö?NQX֔KPoo?t3ɬ% ,@A E)V ֠q-T>O\ؔ~g4gܿΫĞ8vp#Ե 0c D対O^f}iκYmV <7j1rcIX.k! !NYmwIxQD;T N%BJ B<`W|&|O7+SB5rVzBB>G610ˡWo%&xL]$1#1.l40t6;" 9YK2d-_uwflYţX$ ,vC7}gmbҭ.v5r1/]dP% \<5~>qbـǻ-RlYpt}$ 7mꐤ Y_鋣PrOa#ݻnPWN +MrY$S7xxn2)צk_W_XwBJ7!1⤵Ex$;&/5Rv]1}r de_nV׌|'EAϒx0;Vo7I۞$F1"QWX&x.t 1`9Qfzo|> }`3LuBdiK:1K3DQ,TNJ`T2nf$P"C4w|qDO8qL,#G0>$ Y5&j1$ i#9$$3tdp(/kFWN"-y|<3]-f IMT oIJFmrecس$i_~ړZD'jy~gI5vH{vd?*bIֿ:/R*IBȦ4GZ H!FWNP!tʵK{8ȧu}5'OrSD,FB@g=½@`(}/q8Wzd <w}ߧ ?'g},@JXE Wz|8XP6Qh>D`[s>Xo20vl6?@y9%*nv:tIı\3q2֮@Shw1 nйBB|_<>~;YS:qYCe7M&$&T&E{ԄBOHM$ԁċo)>H, IQ4Q)~b'IEZ,L">3;61I -Iʐ7M Lrj5JD#Ͳ`B8o;& ýeebD \3O?M։G0WN OtH{$7SYE |ֆpX҇na :q$#"S$I`cSzu ĪmD;,& $NUZFrR1J`c&3#>fl@SZ!Ix„9JNU9@$$Ԇ%aIB-U #M v3| kԟDDqsV$ 6NJ@xD9s$Ę'&* $1^a D1pikk_@V3i֓$baM>иf0nh}DV9Sdb%Z'(Oo|7jL̛;HC# S'1y6ÞXcWϭ>Gua.l։{Ѥ& $ y-uRry3<9"Y(x ODAFMMApfYSqaY$'D $ i-saYnMn8AHJ>GOs|~=Nu㱀)m5mbt%1GMy-aI{nEmzYHUDga-7וYNk\9dZ~p%1Wjc1"H$Z-D4Sv ,ݲ-H2,D;&Gk& {DV,\+ ubl"IL4??Ibʼ;0܌6f5똹z8ԡiIB+C$Ιv#,N|bLs ShS)$I0*1wjsGȡek>;n?/K>(S66$Q!$&' H7:ŐE&:W\IB0Sİ| Z'_&;aawlLV&$l$VDabgm"N:5< .iHӥ,p| &R Z}Z2Hz9ik =Bȷ5W\9!LN!g5HZ'His2}Osѽ L"]ڛagb䵳 {H}D$KnTӄs+'RN` yÇ'nXNOo+0'j`;ax OD\ $\ԣ#I+L ߈Y&t2ngM O$ &Eǥ#BcyB M O.њplmB6.O[ZHrP/LξeSFVA0G7$))L)hi1E3E±hOCPhHkIyh5BР1NxqbMDlIrvoCd횉w?o&E)%+'j"Iг>$q'Orz0.s'u;"jWv,#IE$i24p8NZ'ry"Ii(I1g*+_&Zdpb!%E5NgIb!IXHeaX:>5hB% ̅٩fD@M4,q ?L*$1du$ %,sĻ WN,Y2S r"Wg$$7D; wָ+ΉhJ2GOPk&ܿrO!bHDĐ58Q5N[KzShwl"I.o$ċ֣,,QC$ C0 S-#F0Ux8)jKIQs#]_H/$pj &ت7 R;U'$BM|zg0*{`khTҡ v  I@6IS oحW Ot#^VtIt-$,絉W#Xpc`DނFQD1$:@ym tu\S~ KοJbj8)C] 4mG},aZ9KWvB6-w2oof}m޿}%*43^wIEDOok;M|z!tIesDP{EKJ%Q  ]9q Tr0R"QD^ $Z s䩁J4I@-tLCx1Th#M̒J Ic|hZ/gJ @(JAp}֢y$`M?He#O,#/G I1ܨ"QEP%1  )MO@bU\a53[*E  Gk&Xndi H$yP<]#5"+# Y$@@_K^3}AnzudX2rPNJ4f!27/nS6Y `zě$[M GP!J! %Cı)`u;~z(&dOpyaͼ @ Fd-5"W_<. LX@ TL 31&s ޒv `E!kA1l TY `'5,gǚo[VGP@%S @C}8ʼ9y3\XBwUb,# @b#" "q%C/ l;qs6:W% J3 @e|E¥J9e^2YqTQY B*1Gx D&8 ,YfbvV%^{FaB 'Ĉ=4jWMp9Ѵj3aJ8B;BD  "t|p0 DF9Ay=@xDĀ J,\B 46w{p-1+.خ&8!/}Fry>&T  `'5,ggv#l%Ie81E.PGy J̛;=bx p`k4J)b 81>KCD&8Ql014T * pG1TmLj|):;CDi$ 8 >,ՑbP%1v#[NjQ$ 8gYpΌuTHT2n̂ % pi:('IaM]q&rXd>jl2w7ex!@L;, !W_93$9>vxX b$Z UZFX椆 u7@K>lqԤ_2#$1A"%R%^GJ@HXF K@LD&8QY ӹH8`#H9^r@b$ LJk#&MnC :sYO F U`ة'*Q  `'5lhP)$ Lp><N|yy T5 ЂbD lA=BꚒ9[*JY Jtr ЈbD#7h#/+IxFȪkpKeV0T # @(& ,.Ias`}s pI;,)"-$]?u Xsdň)Uo~FSe3&"Izz1"IvdjqU pYp΢#-%Ias5!Zeo A!K;JX[aۙ' K7 g$3f Z$æDhǒGF?#0;#T& 'w#%zkI"[Xeр J̞A Njͣ@hZHox9DZM"`|%+eX LG1b8.|GSFGNgAh }ydQ5cgݗ{O<tHYۇk7?حՙLS`HDnTӎ-8֒Y^p?%B U3J\Yk2i\o9ivL'w7{_Ey ZIpm|;Idxm) ^/Pt# by?61Dh#$GDMkY\5<IbIbs=WrUI'm7apl() Ĉ;I ^EAYS W`EH0 L!I@c:9R$ƎD@[;l[,¦Vb!H~iz[~)FP7XOBpa;&D.\O@gXcIsk FC(6d zD!Z0%"4u %RY,8&((7!K&Ln- I#Db.j'Dγ#bHOvpXOŬ)q5D KnhkƂDUëNycU`@H -p m9&H5E8pFSx%DW-4Ǿ Q,j Ce\P$iU #Z$HA(Ɣb}$ ȿ$$ѿV&*e BY#ZHKjMU$ I $Z1B+HZ$A8KQ)*#tN@xLebDSIM[i턃v!I ;@,M)IB)K$_rC˫ۥ>[/ctGE-5O.Ya@C#f#IB ō° +¹ @7+FZ@(RfAX@#&It#$GD01bh#=Rp2"AHT@U' A1b<;H-$ }%jxXBC`v1Y0~>U"Z @`ME1GPGkI"h|sZG Crɠ# WNJ4tC0M1 I¹#@6$L>9ng R7|:T2 ГZapr S@h&szT6O3 dR-R%qa F#uн|}HDDԟ(DQ%`@3@1kK=Bg5$zysq~Qy)at_ ަuJ}e ~#f%#L%b,OquK5bGu"yyyAyDό,pAxHGOn-%_/8K%@7[R5$,OeM\ ˿gP&DB!.IŨ>quȢeAcef;$F(3Ź5Ib,:zDn"Jo+piwE($ WhUw[' HL)+0f!$1*Hϵ.b|!IXzDAv$ $( 7b+N@(5kCX~WM( Y0E3nQ$QgY1Z$g=,Hef)ΧqeD(V?k$2XGĐ5ް`Ĩ, I"o]9Jxɻ$$^He܌ Cgk$]\h-0ƛ%$1*5H_° $yA(NAkI&cbx6y2ht#22 |8I<J>GX.'nXoJP(1b9"8= ݾ UI]1f"tI If夆6pBj#*кKIB -!4O$)|ߦl9ֺrQqxPy0FIbHB$`7(b\$10SH$*MON%ߏFnŽT َe1I1iIz$IbvS"FJǣ3H @$Q>N#I;LKB7?ؗ{K XHbc$$Ibv;Rvr!IBJwz O$ÛJ0Iv!qKhw!H{HCrʎtv a! > I@Q 0LG&i١8H$]ڱRXҔɒƳ8UEJ  I@} bD>E[K6;WD1:$ -t罽t{ i8p{=; _BEMD',#I >jQĈQEP)1$!$$j0aH$;uF+CZQQ̂7$TbIBBi1z%L NmS>Սc,$f~B@ "FJY-A'C@(لڞ-]I-_CDy=jbs@bD4@ A=b@X .F64P k5Ӱz#IG5*C@#='   0@h$ݖYzHd ?!M@>{a&Kq@Hź5EdX'5O0#@8+#I(,(wЂ`lX<d%#T0b#V* $I,A ;nhhqlB5#j1#5Ib|-n FP:Q eܘhK-؁6!$I seε6^Säxu#%plEf^jB\'`~<< Ppyܬ5N-_C`c8ޕzDWܽCEu M`Ǩ%EŲK`K1/oVۚzĪGX5ĵ2Nj]M}X eܘ F}b I(@hx6#9b L;,1b{/ - IL .Abeсz2  % 0(Y\uv<<#"@`<G\M")($U pRcqTnWQ5J̗yб5Ԋ~tɟsQ+S4Gl/0V[l!Xݹy ·ͻ$2kH;s_D$ ]֐$j ~$Q\ ˈҵVYB<$ѳ$zk?VD$#zDܝ ΟpE6$3k!I<3Z@%vb9@uV屘|Dc{ܙ[戞[ IF@h º^}JA[*D =HZL'$#T ]cb#($8IBf'5pa$bI=Br>nsmޓd\$Q$ -3JԲ˷WjWl,Wls H= I5mĈքw$$1s5ճq'IkHbi$ |%I4{+N)Ƨ+\'m@Mtx$z鹈lILEUsyeuw1%bmI IhE@;Y.{%[M5P I,!$F,f$ܖoJ@ А+p/Df0$ -% \+cہL%Ue˖($zV;gu4G#`LI"W@HZhB=7JHkH~l"Ihò$!Yu;^+oNAh@hG";o;ؗ,I="K mUB($$G Ih)Ij[_5($zf. ڈ*1HIIHFirA>1X@xun4>_3oրc0 $v =XFGQ rHYĆ-= 6Ke,|8HBV!Ih6%IVCn4P It+$J\HrIb=Npu,c kC׷?pI1Y] Ih6"I Fh%4[bk`?\ n@$-쨋QQ5$Īz5 ^\h%wyy1W y%K`mg($I5bD4h@MAo]Z?,A@I1B+v6ŕ $n$1" [@HH$WN-8Wiaâ-T $3iIB(I͘Vk [~@$ѭf$#֚_^;s3j*F27Z0ʲ$+1$AsVl%67 oo%!p4Y$n$!A % %!$$k-$ bVִl%6U*]v.($%v5J$$Yj?Bxg.,$E$ajn;mk͟JfAg"$;ЈJerGxZw7,H4=NDS.sE[kI1B+S-l%6AZ6$5bDc\H IbTxӘ%Mp >]Dp}gn,Ƿŋ$,f$vlLlQQe !M4U5lA4:1)?x"LSP Njhe#C7. ϙ}U pp3<%"q>V\>jl'FĒ-tId;Ǝ3}{9#@NjhIR<A81bG-کWz>zKSTҔ[z=_}{U{,+E"DY$E6W@"oJIcm ;H}$ѧX5X8MhE d:73xAa`RW *$@wX>3bp?Y /Fl\4*TZ@ p[{.X7D˷HpeIY04"DLd;,H ˦ 3C);Dp^& 5aQ}sD#"I&}s8a.%8lB$0g'p>AAtb5bnHݪ@S,fUKA̒s5FnhĎE,f@2" p)&@Y 4Gz_^Սb+)yVLtb"Ws`rQ,?1pއ:"\}K >ܳ9W0⯯no@0Eń.6u%1և56T 0X8&0DxbO d.80GVF jd1TM#E1%a77x49փppV8:q:ZexqIZ\޶$ШK,#cpcJFYQ=Kb& u j #F]% [V>f~}^,bq֐$aB($p)Ob#G M@0N)45Z@9H=Zv pܬz*$@35 j-0׉$tybmz>2K{9#@bD-AoIN8,ձa8H;<1dG!8E|swRJs-54l߬M#$'%15 ' aYB9JD"t4-bkNĞ.'ɎrvD, pRc^{h[_z_h+ݿən2fM~#B >q5F` /`Ы\$Fpa`, 9?!ɰwXC+]$Fځ6h3 Đ ]Ԥ~l޶"IN@I1uroDn?; L%Ԏ3npEJE0'I̢4* ID#:њKAX2$vlQĨu[$r_}Op⾂ghEirFݎJtځz:$6ш| %FmG lB%6x$ 4W'P&7\ -3T 04ԁˬv'Ih薋 q-l8ot.9;!I4EgI1i61^@@Qn8(@)K6$)TcyDPYQ(ibuE*;\qΉhQKؔ@P*M@( 4.B(N,|yāDVa1X(I`h#2/'dG \9rc#c,!AhE>DU¸54k5?*YKtJ{&leQNvPP s3~ϥ$n-4}7"7J ,bZR٢[DJHBڝI9&:2NJAvN.z$o;I1i5nVnD*  ƹcؑ$le_E!4a+ O$p6 s&B#[쒸SBW@(H,SΨ%Mn}T~ɧ&S>Е$|l4MqoxNu1$砋-"c% ESn3@r%K ??ǣA֊ l{X#&$Q\?'#8iuUh8U-[ f+ͼKghxL@s# K PoU蟵[K~a& +;-v-l̿ @3NHL_6hb1PY@>`QSQ IlI"Lӄ7ve;a.(.OD%6SH+ABRp., c !)HܥΒwM8s ŝ ed~I:E$QAn(f( bbLO~+VbKCK $s4MHʸ UJ&;f#s(IBSL}f$Q0eκ+iA>A I(STn;x8jJ2HwXI;DӄbNXHZ\ 'S9)#$Z6xb ㏳~дqrX#:$iKI"8E:O-dR_v8$MmqI2ܠ)$v#$rZC8r "-@| zDaS@~[*%a($ υ Cݕy['j\f[h5k(Iilx$#ڽ, I"77Brm$nTRBh %=M\q\D6x9׉ :m[YCd<\HH __ț \ 5D!np_"=kS$!ge}eI³iz uɎmp٬.'^'O’Phw$=!^@*|&5-A@T,@rJ\tI*DEӄ։mpqgZ3e$3vg[srC ּn1a#IY1r (Ǯ[ Iblޭ>$iPvŔ< 򈦵`$K7hE1aD 7Zf1u L$a$;& Wo# pOL$r)$%F 7ӱ0/@?c5oTuGx˨*1RI#j"I34w^ICiP1peTp,f^y$C86So*jP1 EgP80GXWfG*j% M eZ= 6X2fZ؆d4sIBDzP__}O,*wyJ )U#}$T c@vx&0DR +I)MVj˞m{fԳf.o$Zc01\?dx<1 ަ$_:Ga !Ihig=I"Фibda_m0Yv$,$IQhf`,7A$rG I}E,X.zy8K"v KMJgw`_Is$F`[okؤ!$%z5[՗PIJ=,/I4MX9~T`C!;/$8zD e6DpDYq25ʸ=BxDd}>M+Ҍ';`6ƣ Q{$т6D grDgJyV)6@1[$v$4ѩ_F:oZ VHMq.h$)(w=_4QCoչ$cdc<*אEۺHꨝ$ϯ g=NYiA@z=Ig\l3$Ae=fi_^{/rYZb@XNnxX9:{?|8 "܍B`ge;ᝉIT:p@,LMT\t2Ģ%,FRzX-x/z4j:TZ$t\~a'?}d} Idbh I\DǞ!(o~ls#1C#ܧE $1:kv!@D-*1_6 ^ B&@s2$L* MTƙDϪvOڬ@'Z$zf@b %[4MP1 @zD =F1:$̦ǚi 9zUp̙=S66<Mr\8FQ I,Vz&@3GU<өK̜g| 1h}U9p$N.=i#P%6H2!B5ఆQE09N)X@. pXc@X>A X EX.OϿA c Yk(6$MMyhhTU2I Є@k=믾&(5&LZH&v!FT N0BmInhG@5*$Mh(74T9vxv)'=GQ"I4DhJK"  HhP.سD$;QWi $*>'7!@G"݈Gl[Hۦk 4QK|J.a>K8@81} ^.Z$O,c41{UQ%zflM-s@ '"!Ű2(,$I,# 41oמ)@\ ><Ofa #X &L_JԤz5 @X5LkAPHИg4M<3m2xhohఆ\XIBae4M]TI, Ay4"aF`5$1o|&4M̔ˤ@ @LOiuMDY$^к;s3x \q5HXpXc$Cz(,#!|$T %@FPDGĚ#a AI@W/Jt]@h 0=K9ц:V$%'rU" !@؇5uMH5 )!58T҅jxbF-H(81D#IL,\'5L[Ө@GKqXcI7z\+08-FKlJK A`ЁD,1Nj̗8ч3HpR:EUbyօ I[$xhF欈$A NW7Q%6 @AxyZI%O5&K*"II("I"lL'v])!N#bNLIBkZ\!J!@A y0GN!IldB|OTYN]lvp t#KY@!@`M>k4QE$(181;GKv"$@ĂI} O $V\1pRc|-36nJd&Y𸍞Z9=Z0hMI5a&I   jGЛq.5f>@(F<h4 **&!@8I[3@!KI/>\$;DX؉-;e kl |(Ć/dNjcJ x "|o'P T ^.&x(z&WoD'D@Zƙc.jE|{3?}kGwxŞ/@|z$XbNjP $dSq*^XD*e!8x1B22v,|,C"֨ pGk#O1 IY1'5( EE5pUB^ ДzDSsXBas@$Fbu9BPp"#o ="^qmmUb諁W&ݖs@ciuPG I HXXoINjR͉#] Tv5e@H)MPkH^ِĆI Uvr"#G/Prw:]!d@F6k̓+ s@H\!pXc|p?r8?Թ炚1y0űuWmuX~ 'C#T.P1R9X7uh1Bӵ L8KV?-A.~?4֍HAsi-a 5:@fį$ ,IXm@TG!⭻ʎ~8lT"U-/)ҍk@B8#BH ؗqI=ztدvXsōrd٬H .7 c`M@PzJ19$a0P9bX?,%I*C٬YeTun(Y0é @$$L$K#IH(M7f$aJ@!bİ|G KHNBck63wX}VE@Ȫndj7uHhey$AsVGIbX$8 텳$[!ιj}yaݶu"oݼ<rVG$#_@PL $1z 4GiGIbC"@;l:4kn5>$@ I+IBΪH zn{J %1H|$ 1Vx9CO`8[hrv =Y[@$!gz$Dkw$h\V܇} ItX7%VCH6p_:pi ct e&@ I_HrVG $#A$CS1I=qLnJǮnV _G#VfWJaBe.Ò*'S;ār"C%.f$+l8 % s+5xD,O84P IH ׸-s(IGN!pG>?4CPJ58G-BJ8SD1(GmPU!Q1aM($[7\GgJ>QK@3g$ $6~H))$#t[)C(gݝdE˾ɣ62q#na*鹒D H9$a-#*T\@T[ƊeD٬FM;Dhmgkޡrp1 ]qjHi@{JH4HInib!$J 4V\)PkhwԢ䏿. G=o%Q3AHtU(IıO@$a\'3ŒzD4HwnX%I+FjyzGd<>"Ws`sYq8Ph3q Ij@Y$ Ω/$89G,4]whKjy( Ud\e,}4gI"~L-믾lDSg㹒rx$`Ksw `9bZ$QAv$eh`@I $ ;{ҺK=޳$ bEH|~d15V%bºC%+'7b MRa%y=l6=bHޅ5dE! $0$qƊ$!i&51l-AB:r^ϵrN wh9Sĝ<ϯ"I\ D4SU #'7'$Q'wS~oV.u=[ ^(v9ek屃?{%Jg |<$Ѻ$h(NJĴ$QO hM`;I@P*DACIrTRH~k"I(^$ѡꊗhqzDq:T&IPt $ YH,IOj𜡊#PU*zz|A@h %($:T]HN$h0/DUUޯessCU*&#IÛ2xd Fc$b HU]Ebxϒz IdNH &^__c!u}qHe _lX8e9m4VF`$ FeeVW@KXF IB`$TЕbHO.~ɠ ;E>_{kkK1OID ZQK$,IF1 ~zC+`R6B4ŋ!s:O>!XcտK'FGIn@] "Q4%W<2ga]L@atI$ C,TVz,,\w@@HIBJa&$$P1.$Zc`"IhfI"Έ&P%UiOu{ .=7Q9b]@(V8I\iHwخcUn]eI"wz;abT5JEP22FAEb:@I뜕$Ҵ6+nf+dlqKp$q$W&;G*% Z$ڥ|B ]X@.$\b$ٵ$qGw#Q+yEV2C#$q'Iasʯr$Q<1E B7g B)!pII_a Ico~xs@N?-qq =pKK{Ħ$Ғ.M+0x Hk*!$ !(aH ۘ@8pssg/GnacAG.#.^~% \ڣb5B$vv#$)@-$cvgmE/$$cP%j0|.P/آ-$#$Y[ Bi,\ ސD#It] DWF(ι$!&C f/$ $F"L-$$.SQSH5$NDh$ -kX$deHK8,b*bY Fȕ$x%Fs$3AQT1<"$щD^sS)稳DP$hIp9KMJ%*(H* I\GHjp:Pɰr$ˌ%$Hpi]BV|{y//[# ڕ U(D,#I\ڪLW=.uBim$ s/RBZ>X[@hT6"IgrI-ja3Dk iHMD.UҚ0%T$Ѷ$m`ݾ$&A/L.ƹE8-l3g>E*HdvB<^q阱IguO`.Im"IeI#$J;F42{HIUzzk!ILS%$ H"Z1Dq z6mJH + =!ܖmlZ29\`+-$1E0ax͚fcDZJH!7>V@hX~H 4mP`W2׶`.v_+"IT5sȗKcE7f&ҚX:iHSӑ$miښ$m|.v/ rh$Fg|}%ٙTa&8ҕRHWW)6V I4&DC-Mۑ$\A8_$;w=`$..!Ę*( @ _}Gn*Ij,&$Jc$qS䥛Lm֧6$|R2W`rWn U,DB@$A}9I@ % Zs^~jym!1$$a+hsHD@AZp,$A%n UXΎab}8ϸ ZP"֓$BVKg$T1cy+$^7wn9A輥4wgn;;y,u}$࿻T7%ak"W_. g߀ %P%  IAZ`e6c6߫+JLN"IdGaJL U+5!N8d7އ*HZ$T0[,e-1._.vo"IHeH4@.jq *ԑb;׈E7tU vSs,HF1j]T AmZ$2$A"~4&T0 <֒;^R䒃hs$am_gM,nMTAlmO$ž<`w#k.a$љREg,wGlcʶ$@(Fgj$&TZs \.M]ʭ#I∸]#T^NE4q &`-OAE^wzI#t32ڨmc:v$=QVeܘ MD1rc8_@ȅx$QЂJI$0$Cq w.wfS) I̘>sdXRJTe&G7]#HF`״LU7?=#!$h[gY IN.yBό*fZhǷS(YU$2U-9a4Y-$FEAD;XޙR_ FnjJDHSFȭBs%-ڷ{kͻ@iu~yI$ vzXTᦿ]0տdm H\EqӿmYۤS,[f-ڈ '{(e I,ց L&}<~y{QA*H6oKBX|S K쑦Hha IFܘ]4^f 28pf pD*1HovS($D$1. މ\jxy 'g$1{ODHih4ADZpߕ\p;H \Q`w3jeS S=q5H22?K̒)RDIJm*uل$aj39K772[k$t2qmI"qg//\UUKTc2XL F2!ImPf@V$uKZJV]h"W+bΒD}"L3,E bTY$Z+Hmu/% Cfm,-L4R:ϑM H62W.e8 Jå 4 jM@H+WQ IhU&vΒMRS1saӫD9,S$Lcjgh:}8- bh%H|AA$ 2Z`,IpyڹNGֲ ^Ɉ. -,I$.pvwIrpCiPGAA W b IX?*HT?*1p{J^@hT~K4Y,IN/w$ w?kUsO:J[ o-4dֱx1 IL\E+_x #v#鐸 ɠǸ&OG|pHyF%@'4-Y#ZF0N_?;{@Nws,-'~āE8%zi"IY-0M] d9XS0~Iq\;j" $T(mvYx;pw?*A"IsTP IدYg p>U{2n% GK%P%UWlk/pfN $*0a9H9Hja*q#Bx % /C5eܣ]bTpMW_.J@((g}LUq;4;[e =@`:v ?]t8 #ohg5蒨6Fo0V2G*kj 'Z#0-XK" $$8 `FC6SWD%m鯺x8(O_ݖiEt[k(b"n$nrn'^[so۾r'>'"D\V:_,I:#eS !^^^f$vh!o)fǧ/8ͽfvew\!KgsBRoҍ%a9\0a9;@  LGsӥ !PC:rKEP4s@ #@~9'b@^R@.n%&$!gi$ e @ؘ'w!PyIEg'8QC"@YI,\;N.]B=SHH rC % M$bnts+eX p&P&e$S„>S,B`'HUb pC)!4%ҋ$E.h lOvK нNv%$QEEw>qDU USذE$T|#c\$QB]߇] $*9Y pf '\vH5E!0 [$IBIEpy,lG!Iq31 &L' c8a,! pc=]7D4[S`V>f'S\'SBH6h&&7\^"]"$pn˄("3!(Nm,X‚]bzh' IBHqXA 039{;>I(.!);$ pcl+E$TV!7 9%Y!I<"o8OK$MC#.p 0@$a.;ĐEE&zPf #@ &%+QA %"IV-pDa@h@bLr!r.a'$̤#\0ђ.!`-!\'a.q3L|$!M$ lsoS:Ӈ" @7|qt N"C1 $ Rn azhT@:8ǡ+؀>H)D.1 @@Kt"XHu<ݖ`0LH $xB .1>KC.Ͼm.A/Xql~ О! ^~;+nvn@r|H@`Z$g'~KF_|2}T4 `./. I gwE#&qW_Upcv& #w[2A0FsP&aă$~|Źr# !(ָH2Lq[˓ =>YX8%Nev%=`JX&ZP& }ěU©}0hX2uKķ*ݸʶ_/.wPOBFJlmAp n !0#wLH9JΪ`oY鎃ƇF= a]7w5;ij?{WcGߜʻmY+Sn[ܝApn7IǸX~'+1fOI*6;>+kpZNq8VgI $7HYE,Y K>d'LJ$}xbs EM2U-*!$c ^NA4qݔ@I vn!KL>^%&;]#K"^@W#*o?eGo~oǑ. I=8<83|Ǎܖ>D $u$q9?p3$!IA $ I];O|x}c hB Ј23{K"] 瞈FSiֿ8o- v &*kK I.q{.I(Y7Z$|pG8 @=%bgkqM. Il!.Hb$?8{xU e2nZK$Z)cwVH I<.E;$|\K4$rE>7W-lG6k ŕbt鉝%;Br4%uxbwh6 H`II«s]{ ż&L%N}|Gpj>݋|@pJlj8l!pI5WŢ< EϿ);v>01G.Yp$Fni%e;+/ȍE",KpXnE`.Ϲ]SVM@h@Χ$v#zu[ݢ&^,O? '%|pOpx{DHd@*|L@DS pb Cq I< `B"& @@Dv N~De3=> d T 'pFdj.,T݉;eX!PHr(m)$9$@@: NmP@ .~N6HkG9 =@#9RO`D2`ŏ8A)@@H !(0nH(18!u%$T>~V%&U XqA.ÙU,]B v N"IX>p@IHP  P@vhLٍnI%,<"Iuݖό4FpEl@ R,y$A"9[hhhAY03B`$LK Iy!g!uˤ@f'Eg p! 0H ܡ!L̝? @El@GKP @Gߤ ЈIENDB`PK !ٳppt/media/image5.pngPNG  IHDR&>sRGB pHYs  ~̑IDATx^?9V61 UGHkUڐWƢ j@-gAwYU:=+[2d c˼A2H^/Yd0x22y꿿Ac^yXOnޭ#~AONu>|ոL~GS?soIMV/_>Y܋>^m.z}7,hB @v0^-ߨ0Y% Dk="V"#$ @-.^" g   !U !kE% L I&E,%(4"*FY@J.a$7#$!i$X~;P_.hԧ ؓzĞy$j@`+Ӎa?Gx8dD izg4Xk@X#DlEUl#̦U&BSxd#0E2khWX @k$babi!9~Q 7%5T -؁ 0T #iB0ܘ D`=i hXxmBj !TIG˟'%/I},R2+9!y~/ `G\.A-C_rؖK捧;SI<ҫxN7W:C}Ɵ2R,&ά.F UTd8UTdq32:8ݻ$Hu T MˇE ]H<`s4Ob*Lz~@~ m ~dyQ`/f_}{XgKYXqgN_`V\ w% ݎZW 2  y};LyA8G+g"t:/?2W'A x=0x tXw~|p /z 'ڹc""\ÜW$t($'i[w݆?Yoxbt!H8 {r\WE9D(x #3;iJ,IR _Q>X2o˯ށ>K!9= ] /ܻ[\٫0o7o??IR6 %5r""~nk"H4_=H0w_O?ɟ{o??6ߒ|+)sE^C~S  Hpe\.o$N3ɐrɵ>)IĂ;}Ly$}lpS^#.USM܁.=!I<~K"aԫ^W w:oJJ~DK>O}qw)CDiI_6lwυ1B e,DCH|b8d.>w?N]- k\K‰r >I/ {07.u!Xȸ{ܩ?qc< >ܾPѕq{L、Dz[~|wc|x!Ko)1pi0ELN;x~Rl0Wrcśo&NУtWi eӞҺ1˛r71exIxm/sytctrˇlj(&]Dp,| !ʔeo$xE8C=e[ģqnR|"L<;G=g9VLЫ~ݻ{g>?]`![/iwZ_5 >ȋM/z28ɀVdcMAh\H\q'Z~ 9"MĖy{?Ï_GAxxTF$o| |Uu*ӯ{=BnVY6_+)|n\wyҎIڄc pe=Co B|T2#-+ VN?tR#ͺ026%@LO?{ߋq-YBMBDH"p1>أ5K2L8&=ݥ !IȷH=|HOĨ[½F7mڥq[yQɛv OٍĚOФPsšAwGm X6`rV;wNt RPСMo#/y)ıܼe !&72pdY;c`\K?va rpv ¹AQJn|Ysnt{\5#AHD_=f^c HtFn8ef{dw FKkfP(8U!'Hf= WKd5M$b|#m_M 악G"xgnZ(+ ȹm/~>Qq,d֧Gӳ`s+$?K!-+~'Uj fs@ݟ>ۏc5'ev&5Fbć(-#+[Bݠ<Ȱ,[qhKHv*n 'O[-Zĕwm2KG:ܽ"aJB*_pn,sY"ť Fn/~?E>^3rg~V%* 輑Hbѓx`BL?~'.mt-_\]<[ ng!qe9-% Xߍr&&#F\I Kg|B,["|=,$Q%$_#5E7g"ػ 7aot=O,T/y܇g xaD߰Fž_b򂆃,~nδS<5ͦE r[w?#BGv _ț?2Oكx|a󟘨tvK zD H;b#,7߸yIHp9&by"<{Di 6=ȫdl:^*G+…ôoi%'[\Fm"}h$OT"Dkǭu$P%eD~Ǟ FѺM{%lvIѨ:MqT:8ldn$"9\31˦*'ITB Zzi(z$II*aP@YW55n7ğp&s4l1h@8WNp4(LB5NppLꇀMH6W@[wR忶MnUb> \bĒi%eppC']:(m[oֻOZmQ'Fmz3ܘ7ܘ7w<=14ڕ 'SLǬ7 xhkDT"eܘaNGV0"}CsDLp)\ݬKe+ sC/M9'-y76nL'^}?jvޒ M 0x ,@LQ/UW25~d(?rkO:z7xيrbr.EF"}2rJw#U,F. V9+<?FZG??$9->Un\**FC8L**F<^ihJP,+nXotRCt~Z߅\ΈD杷ޱzOj@>#j޸=wΔbrd؛]*eÕ:QJ-Ԟbݭg:~~=|޽-ΫCHcgl'\VYw<;O s1^ ^u8<:4rp&F?* UY8?dKN#c ٨ p0P .ݸ4rD*4t/c+uI8DObuJOӊ1'(K\܆l`qcx=7 o9t~W O!3'{C.˭ tpfpV~!1;ݔ8ؓ;-^o6$_%{gb]C{w u0ۏuIc|3"uP<|=[8Փ'!o k=bV2{k'g T6:9cv ¹_z'wɷe3Iy܅@x-_nl8S>cw0].˭[tpfpVx؝nJBмKwI|#V)O? 5$ /Z_?x=*?cw;Y5|nszc.~ put#=[8 uz;\Y?/ g퐔r9'jp]e`K'_*g.E🼥 Y$rg?SQ_λՅ+ - cr;(]-&ޱ2?>uvOd[D<1V {}gY.y^^9~o X:rWp#T1g%)K%y)eIčk)Ǩ!?f%J]k,q?i&K?:U ᰕo>'-k>lΤùeB> ! f#(_rY5 箢An ?uxB;Ŧk+g>|(n+}k7:L! 3MG:)Cu%FY#kzӨ?+! GÖ+oÏbVԏ;1BpnH$ ooU,;~D<ݥޖq|{SsiNPYc>w/7_ݳ@.7ҏWI *%좛>{ocְx3c?\P LJpp;n8k45=BXn8\'zFdAn}pDzsxњ?<";&‡?Zv/ {Z;2oo$Kr:`0e 7'nYzlc<\>҇Չ{6 F#v.`97]zd*YgB+iʫ]nO Ktppn0FZ^4 >ϵ[G1N;)DSvzp| (ypA8BPa*nuu:܆;&{aSB54/4# o$D|Ѩl$f$F.X/+>\QR?QΧQϞ$C*\Tn85k]QjyET_p^ i'!Ed'ǫ!4hXؑ%ˎn, ZtڙY -(@r-|zWtj(oZԝ5X \P%ZۗGZ?RǮ_(XH;wFWhxll4,pbK\.]vˆwbĥeYe.(6>0K[TvX?h$Lo7_>& PQ .U'q 3^KΧ+MW%J8׻7QhMX:|4#s7;`-K XZ{",>ZW;j*!ٟ_~PP7b<:̊H ~|s~lD6=2h'nK+X-JLAÑ$Է-uV8hLH,}0" ܏IS]rYQ!3sY|jB /L=Dw\8~`@_8\h!^Q]ͬ*}fϓo$$F./h'@Ac#X$c‘XNI[agQ>O4Z%zć*%AwdO_-a]/aI,y ?!-Qq%bY\sdb,Q'qOXjpT}>\cDMlo6̍` {V5+WIr-bvBe*׈¡ݯ#Eϖ ,Kܵl?Y[Y3p<]\nʭ'n `u\w*B|Fh$+;ͼ`_]"Q鳅,DF7!?,&Pv&y4@<{n}f0.Ù.N910s#IR^oBfFsl]QHfFJ)H\iRwq+}xp#ٔ'?FP%J7KhA$t^:7ΞK2Yߵ)S*ݎ:qe዗}Upe,+'j k ݈cnpp#ؤPdŕ0.7~[~زF<u睩÷X|>8pxZKuޝR (4g52!t} >Z4M\Vv̂r K!Q_za{}A+r$Ap2I_J 4-'~1)|D,PD"g` J7kJ%ÖXrEbSz Bs/?ुǶ`8-5M1ADq9yD܁o0;\zt7O0!Y6nS?28Nc醋KgXtP32Η8r7E0w?Wu:9ïn5|rzv;+ e˃}>_U@1D7G:~އؓOǬIZ!'}RȬ$ Itm=% C$I)hcWeЪ{cBB.S(.  wRʥ{% 7`m$ k]~C6q[f!~wms.].;|ȘZi,Y:ѣ4č̅-kpѹߣ5\#v#|[ߐ# -ɕ3}2Y+|# g-Ʀ 'L+w/*<Ս,vC:v庞Oj-GVY%,  >B,<覉 k$ny|~x#˓;~ C?y'JNf#dh˾uzK>ӽBך0FøNfW=6͐TZP͘?_^77/]s\=#[xw-D~kU%Υ]`r֥`I˾u]51jKғL,|ĆQ.O}L} V}oo$"U"0LoHt؅wI|x/1ܳ-]`OEcr}GwKi=u^Jtϱ0$oI(eU7$QY"_A IK)PF+Ih^#܈i*8cDQ\A( aM!IYg3!I4{0hHѡӚ΂D׈ZYD,L/Uj$ Ika²$[vq儝\XHVӄ167Y#Id28x.I`N \*,"j fyiL}ČzI0aG ƌ^3 0/$fz =I'kM!I̲j>6$rJdVd$Q!ӵ$Xp+^31P bHM]mljpe% ibaI݋e˒JDph퇻9s儩tL H]eӄeaIˣMknۧn~\ݖqUd!\FLٟbV`$1Ec'a!/Le9% +щubd᪜^DR#.̕'`$D4#SH(c~xlWE;8"^k tBy˧d}$6+LHh+,M̒)Thj& >IBR6nj$na#6u _}w;Q?]-fgIǒu|?I8pCB1MtCBNxl{$,cɚeeeI֖H{|6/܏mB&bks_*C$,a"-Iap#M+'J|*K{R%I|ZPoyunK?Y) @g=½o@`(}/q?B c p|܇= 8 "_B# @gqA).) KBi}$*Sr[SM;{6A|ޥaEHJVu3KiZvXGSO*+)WG,}B$wgpOrdI)ib0$1υp0)ܶ =#/GI9vĪAJ@,ݟ@/yEM\j O%H lr6!(X35{G/"Z$u#f& D[2$84M<ނ%땠8EXs N@9BݱM:BύC_FnN  ̃MOܻ^/C>w^B=AlJ:t9G tUx ezsr$ o{\N2$$LHB~T8s&,*Zja3+*2qCXNܫq$%%QV*fYH"xVH1a3~.cu p#<%xpNĕfp2#i}d.sq,!4v&0v(eʦzs·+o> X! yhXG蛸\WLq\\ʉ2n"{_c)I£ނ0U;AX5책xDT 6)cyi@4GpMxy1:tGD N+'`:rՄ7SEfJYP8$M5ճ\mSO1}nɻX(Vb#5A1$]J\9!BΊ pDg,:σ*Mx>ܞ<^!8 j(6vJ. o{:9p-Zh]8u rH+f❊SrDdr՝m#$|Usi1nL*1,ID̕Ao*4-I L\F0&4JƖufq:hd@\xe\ȅ ItJn'B]ISF56 l~8t[~ IhC;IZnӄ7Eoҏ E։,\ ItDӄ0$5u{wӷuƘ=1}$u>na :q$#"SV$<aO:qt SUAX5Bh6eN&$dt]&7 7ÏϤ iIOmh$Q)L &ShU;v&( !$ !&&j $&ji}-v-m7×F,LM I;I"&~SXcS<"bWNY9ID/52aIDF@g 6!Q \Z&$LŸ}BiO? ur")p1Ɓ@g=­t2z4+L IL]Sy6ÞXcW/ֳFu<ƅ$΀=% Ͽ^xIE>ane mbt'E0Z g"YF*v aBjlo}IAW9\$1%Xx{BL1ыԂ밐> I iB.$ w0Ua [~MsI[B U&5kWNȋBj瑴Nl}$9aIbiBj;o7G|Wi_$aH}w?NpkW g:a 9Ft 9FMq$r7M $*aBjJ7,;iIb"q~%q,]aMT)&zwpmK I_z# $B3LC!K>52 K]@H„WyD\WNHqY̕KMTOP&Sc$e&f&ۯ6{/3[T6[r$d 11 IB&y}wy؇CJiEX!wUWb  duͰ`:6cwh@0$j!LrBf 95FM,G$!9$n2yNˊaD)N@(ȐYH m窭gu8xA6Ƽɻ_ y")䬦IiB"M%N:I"\xKrzj|%D "LO›BU^6$zx$ ]Oso9!I:1hBt'I O „t-9ӛﯾ[Ec5$1KU I4T ce$ X6&MϹrFHBB։O~\@+[@(&=qvINx'XqDYV$p?D%ӑ$@1v:aTr]B#6ѺKlI"[>~%I@uZ'% )$$Ҹ'z#„{W_ IBA$ uf ?q*b:żڄ\GHI$/:$ 9n 9FvFܨʹ8p:@¥$V6Eć pJ:tDp\#H'cHOk[a6xqZȅ%Șz@FN$L8q߄OlTTʉ\5PN#KIv,O8k\R UO!PT#fIbzHbTkKwz?~6f]F5>,NHsP}5<Cx"}(=MJ 9([gU@_HsÉipDXr)) j?C(V>I`&tȧw?}+3QĨu-& I~$稧5Mx8Y$eVMHe5)CFyB MCCժFv`lH}8Qn퉓 hkLïȕ$r_.$Ő'걯*Lt sX;'z@0+Lڄ+'z[)NdI YE$$b5)XL \MN+"I(la IlNzDX&m3JMU^YH=i?ʉy rv5SNzۋI3'$'}bhm05$4E' . $Q$\k< \2e5 ]BEt/8$Iw"I&^,O z4WDmp;ˋ) MY$ C0Y[Ixˋ'61KgNQM\J#BN$z7Do֣D=0/F()e$ʸu$  -&LMPkXS\9qVZo/Cnt.S$,#MICsD`NDCM|zvS„6Xj8WZIB d3Mb=U *} wڄW"zo/4$:DgF7b:ʡ ֲcP!EPn IB),,LMP)Bn~m%С|"X*{`֏HY6'ϯ[6܇ι/ p!oͿ;nu$} H@8 G 5̅ xa"h /Omyb\/ղ?$Yk/N@HhsW6K"L(8 'X$Ex"4%1>k$CC/ˣ:z DoAp=oKwٸD0^@x}}"7,R]p7ҡ7]]:*F Ї@,FY*NXynzě$0OYO)YNQ;S'Q%Fu!KS0K<zě$T IM0f,.˟JkCr7 @Ik#zf|K"ŀ;99FTO ; l{9^oID~hLLU3p X&a9;}Gs qO+jc"% W!@ F4٭ۇrc7)GM @6 @;˝Oen% oseXUO 0, @K;,_fZ$\$ J L,S1K@؇'5uH7#D9絞cz F @`1b?7+9= I"~9=, .$ћ*I 6$P)FlH!pGa&8(F8,f@!!ކ2:wXn lI8vxa='zyyqV!, @pe#Y($09_?yyVQ~ HPF`s=Vp9QVyU7ޮbH* pe%@@H&Ias5mp.GT9w@8aX^X>@f plzD:у% 8Vz)ryJ$P%:g9@X'5O1.@`$%˧[@hM ĈQo3ji"+D&83#l#*1]pL@0͝A-Iy9ILSJ,DB !f:v#!DmN~<a)% J,V vò[,C)D&849Q H@< 9axFB`%X)B<% 8<=3pېW߻@6L!C@k czD7뒄sYI0=d$N!!@f rmY8ֈzD.9$ L,ycrek!FD B6Jf$wq->.Teb #@X>@B5=FFna]Z$ 0$#D(tk0 2jl2wH!@bazDY$DY"F(Ɣ; I1bϼO5zDq&$0A|q%T 9C@@'D b[Kچ8Vą J,\B?X#a-#SOzv$B&8̮DUbl  pt)G I|BU:a n,~3qO{杨!%cz#zH.|cF=[*1<TSLआ\]c}]=B L\Gp.OULK?{.R]<T7X `b S8:IF] #>,St洅Z2k~UBhA\cFI֝+;kx_q@&-'MnspjV6wx{ga1;x ߴ3>D g^2NvG) ^/hU#~xgmb:0dGH@h ^Yy Iq2ks=3J IH˻0.J J*z%įjm|SDx58{Z}p蝟m >Z&@ $ eN#LʥR~{oV856*QZ̃ 'FqN:@h LDˆzX@ FLGđNMJ+ ,@ 4G,MB(&@D1:H*hbD)t)ňCgmTe Y F̚9&M4D6 8#!# WNJ,{ւ 0b(m9 oٺetg!I򼶦r#W@Z֨#`˺R% D7X `wXOA`CH^|#nQV 0>kvuh0>Bj#wXfbp Ⱦg.-}8?8 @b1)TϭGno%QA(fkJFBhVhv=\~WM( Y0EXW IIbT^$F.bͼ^%1X2|ecQj1݊@ zFE0,76$^o,JGTEx=uCtI;LD"cS[q.s4QCQqxС2Y3$rHC_1bx @(FW?I!D9N#ICB'cY@x`eDZt@x$!9E}࢈,$zD$CʼnG(F$.mǾ$!bm"; I/3IbH*$`(E^q]$ ErSrVHoTJ IhD͉\m\a_`<Đ$G4GghE$F`/뭸$^Cq!"I;LD"fc\VYWN! Yq+J L/ $Qm)s[$GDP乎$UCq ICBw$+5"6r,mq=vIā3YPT/j$=aIb>"Fy$$O&I;TI`ND=:#W/&6F 34D(*Z.;HH%$$1]#IYՏDg[0'IpB7kHZx$jHN-Fܻc- HM$ ]BkHBP #%+o^^\[H4;<&$ -H$U!Eq;L<aGAbHrV#پ*n)hDk>zD %K{Eܴ$EI!n{΋M9 PK!$EϼъZ3$-#H-P7$PZ&D1D$,+>p$P,߃F$D+"F#lIOv#qnJXpH{gh?Y= d9M!E%k$y$1oU#19"pID? @`gNe F#cIgV-tFH(3g!<A9"! Ia{90$PVK4PTbg: 0ĈRsEb|E=)NjHI1N@@ц l@@5s=yru)I$<@Dt HkHE4@f p2#H%y$ \@2 4#~@; B}peγ@}v̒&HIb |@[ZmT*4PLU&8 } pRcywHtGjA$Vd 0K'`h_0x FqK=b,Ց$tyb  pRcp]r a7;MG,h$J8@7#uX,,(ʸ1 (#eܘ@ @ 0=ĈC Ba@apS˩ӇEby$^Nh/;r\$7, (̦ 0Nj̕/]zJ٤Kbl^ v{" K]4PTTS!/Ĉ}sOGHBhhJMb|e飁 * t#$ 5 A@bDL$ lGx@K8J^___`Z$H$a0) Eg4Pt(/D%)y9IBg5$Z/ I Fö2btEdaVP,/O I,9ִ!ܚn$*% /w''\M IZHόd#$#dIŮXPAy,fG.@$ѳ0MsD;;[F$#Zv4Wq ָ$QCO.I% `W,ب؀ $Ǥ+@PL!FMOIX@PbB["(n#I@k8IB {9[;˕!ۜFDBХkgHH_DY$ $Er4V>$Z tvS֔OSӕbÓ6P I&f8" B;h D#] \3EE)Ζ1 81r|̄6 BNpTR)n(@ń Njhe#C7z=9'5T OppCE|í}YN%[6J1w$Io/fvrG>I }X zA‰kx[=чHИ Ĉ]2=O O!B ˃hynzXue!I&Y p嬙[ousKdaI)@`=oA5Nt 9[$a-#3'@MRr.[$1 BR3ֈCzKIzK-e[rv&1}[S}ҷa٧X3{r_~m߸NʲO. I@YHZds% -쬴4IݶP3$'kKJ}U@zāo@,Ku] 9 JHlW$q@6Xz`ϧ8>ň͟Q_@?$*2}na9w#-AqmEe4"DNd;,H ˦ 3C);D ϕKl{Hk~LDZ/hN0$њ0आPD p&E6&%p>AAtKb5bnHݪ@ 0/gx49_7}gEUlc ai" 8+hԅM #7Hb"vzO Qb8XB g,,#Y/F1XL4uӷ 01N@Ljn,Z.\>rӱg /Lƾܳ9W0⯯X7 ObBF`kwI FM;u,CΈeRI xbO ƻ;\qH{hN0<{AX#E/@-%i@Xj>Dz0$6['s𯈻wO|6+Y69❔*$aD$l#& mci6# LmM93$Z'b$:طF -7\+<~ ZrHjE$YNP2wjN1Z'uhg3lǓnB;ޟ$Z(I FN. G."}sƊnQi.I6ApG:„ݐC?.jd3lčG\l~#6"II1uZ߈pn\ A4JfFh3GME)IMkyBg5 a >0 zX@hD[3 TYDP/5 tXUB>Aǝ$ᗣivfih6ayfxCP̂5$:}$ Ĉy~O̹ Υʦ]œFTShV6ӒD@AČU<-ktQv@rM:IuZK3r%۬&gpt>bzVNeǮA$3`J(IhP >';hSaRp HMT@h }BPٮǥ#,gIu&le1Z'rhIv sy]b:'BZ&_w?>vJ۵쵓$Y^1"7S{Yb—G@#Iت<o$ 4M*)hyB ~ϝpϵtn.$.w8i!Q.@_k{KJtIJw$"iVRdG턻-TCc\ s"+;bD|a VKx&>S^-DLTEqI‡DӄԖzCD>;>og 䢩'wIL-I1" rmv့ ƛAe8ky9qHk*$a땢(Ih o':;,! 0(YΎow$#Ԇr%&4czk_;Dd[nG \?'Ii2Kˡi"n05IBSlxR޺xVܐ&JN,g{և$aJmaP]v;vpYJIgg7l(hP,v,e1wg1},FVF܎5bH l>&$Nxƍv̶a+@l $.#[[($#^ Y.5AAԓX[k^ρ. \()IwiB!s&M,O ip4&MöKBbCYbġ@h߇OlzHa$iP4pe;a.(.IDlJ'mKKGa'St{X YQYcv8q`$ѮX(IhiKn:V41VL4cIyOkB2,(784zq7Adi_~?)\ (mA~«6+%1< Ixoh0T\YX kiCh=CJM-Ijx$&#^|jfi$R.*7]|H%$1i\Վ$ܣiP}te1yBe'be& b%Q:EB0+F 7hڨ9<;"# mSjH#D.13 J Mfj# \Ki1S* DA 9҃$!GȞ*=]V%.$,$ImM\iOlD6`J{+!%¥!);5{$ɋq +7[0:Y IP+Ix4M*'\ѴXr#$юmN%#bb4b;!rpCl;gvnH:$Pr m|+ ;=bw!_?Lbnn ᧣J12.a$QCe$ ϔ C5ȕQmp x, E IQ$mjKZ]r} Iia@͎r5KJ ,GXiRdC)OH1C-8I$ĈD"IAqh'7\q cA$%7. վK5@%a$a$;hzY@x\S9%al$iJC~|(8$N.o"AhFDMq8'R@HT>D ,#PO.[nYŨ$ $4MH(1ƿwt@IB$L@nPLD =-5̙{I L@蟩$䙠iBjۑvR$a'ܻ okW%̺c8!t»iGAƟ ]) $Q)xa"hÛ*01:@74#:g3'FGXH&Ʃ 44q=#N]b qh\FӂAh^p1EIT:pFM W4*bV  pIpO0i Bhah:HSSh>P%( @pzA&-F0wץ⟽I" #@|9k1DK؆ Еb k 4"LL&@ӄttU#l H J3n9X&Hˤ@DhaZ}D C'mւ Г-=iV aŊIbMRRchb :@ez$^N(MU \dJm3KB#!kCHK 4K$AEHX>qXc"I,\B&@D6%&J,F N#:f81Q-<Ibj?4Mcmc%T y @ 5/$SLh(g7LT 쎠{HtG$H$04Qoɨ O! \0˅ZϹt) &$1apy&,DW,@hGCOFsĖ@ |M$MTBpL З˛ ЉzD'fAdi"LQ%fB@{Hg qyĞE$gމZM40@@}ULz&ӲSH{{h9mg UbƬ3 k#eĠ?0\z|(zI'w p.   2<v#a2n9^$ *Ԩ· &Ub$ j5eZ$xhRF ,&$Q pRtzpNFUBƉQ 2zymA-bD =Z$`EL4 TI <Dp*kX~!I^+II,@rHYm5[{`$ű0Jx#I!4@ GL8a! pGIژ'5O!8Uv '@`m9Ap*;s@$eUK,I%HP%2`1ؕEbג$HN^e'T=N#xhYsX@Ϝk pR ''*1yq А5tH bNj4CLJLdŁ؛]Ebr.%Ԙ3P,C9>!G 2dz8$pRcV-TŻ@w=B>f3a2$2`)'2I= lPkl^;$C I C=8Uv=RMhX$p{Nj̝A޷!BXk-sUBtFB P˵T5j1wH;d/,C#~~RO;ANU>1'k H2pÐ$ZDhMnˊ/;$v{3g=iNP ΃흧,]mBxg =j$ BoE1GHya< IdRB89B-=Wt7p w )zӢ-6x#Q4q{xۇ1HM_YJYɒnJpXCFBD1bDU2O.hyH N=tKܔIlu"Mx zK<4' $ I$чsUI4GtN%mKIbL% EScXXY:#{Q#nT0FH@6V en֨$hhFzDa$ cCEG91a-[!ڈrFp2mpZ$$o@*0-;C$ ]Q$84_.ߍQ+KR@3Ү!wNdԼ(B(fy_  ,Yez HZeIB;v2'ܸ!UJ$Q3a Ɗ*vh !Sjq7u8m]S.I]$1&/|%zDВ$#:$% /I$X.\Ib=1blC";hRwet#i-q#n3k5$O@$X9 ItgJU#U ڂwI`C˒$$S-hfh~qnucGǴ=%dG,ڞ- F9[XV$[Ba-WۻdLGo 0%8h@lFIP($lq$#zfX@&ւDZ1= 1KC֫=" WϊOBKxeD=@$a;HS_ IpXsXi%Yxp6V Hv3uzDgIĊKBaƊKI[CAI I@KwG-| .c$<@$anp1$BICâHHV%ʓ< rD yzm'vkM-IX(#[e$J6?4GL^A @x(c !o@whE;rvHZ HK~;IOLK$ 5VxINEa2J!I{7o-!9;_Ue5P It%Z\'Qw5Tb ICN^__bDe\#T6?\@M1$q IOᕭ$QKI)7 It- V;t%Gve A_ %8 $ .I"155 !(A3$~$Y IA9D˖X%m}W?Pp8&Fժ_RE][B ]X@.$\b$ٵ$qG߲MKSZ$dL#1[IIB.IN@5zD \&]ߜ5 u%$~$яuJH?۟%yݼq3'S!@ȕ$*aie7?$Q-b\FT!0 IA) CPā랛=I=r{}w $Y?$N̑$:.ZfsI} [׍`#IAV FP$|r&ZyVVDkaUl$$щ6D'ХlJlKO0$9>nlE̲Y."IKAG˵b4*0#f金_?~cI܀#$ѩ $:.]fCI"k{Yuy ˅;/]6k:[]$F7yx}]j$$#h\yfL*p䗷 皗zDs &I,ܽrOrHZIG(/ЫIb = BrT|m$#/H=(׭$/Q,9)`R0UgH% ]%tK|UIu@bhJI)Ƒ$zP[cIfXx9HxsZ^"It$| L(t7\"AbI`RE4\I%v$z  ' IX+%$8!D0$p'U8o.>/D"I4GDEz 綴fX%^#qݨ(m{$a0?KH$ yMhD$洑$#[`jI-V!lER%X#52U' $L]$qYשNhbH-Iw6$#[`^I͛^YڄVZvIldVT I/q%wߊb/f܃YH5A<=♑JԋhZ1j9'e ]2G^M<&"T!IR$rvfNJmkQ)kSսs)[TΞ6$f;IM>Yf UĩAh[Hm6>$lrb CS7WW߿ĕ{J/>%/s_Vh<,qMD %@` = $%$іoHM NfrUq<JkեUw$f/┵*ND bX[@h[oHm6>$>Aߙ\pZ،3~B>Ĥbf*Q\B iMXC@h[Hm6n_` &"6ϙJZu$y3KD!U&?D tؤ'0T$Ѷn$m`ݲ$ ~krpX=*=Kk$f)3Tџy .:n)z!RDۢBh˷u6^_hT9̞mrJm:%$ ͚F" uH!H28V@hXl 4mP`W2׶`.v_+"IT3aN..ٛ"UX!D~>$*#I4Ҵ5Iޯ%l/],_J+#I΀Pg@ƼQH!X$0H 4mGy~=Nut&y`{HU b$AH!I)*$@(g7tIm*x[|vs8r`7Ib$v^Bo*SBRDbCh{1 |!=W>@XhX8ǟUOb}$ qB"I32c$HLwX25R@0z&kysDL!I4(If~mporQVmAh՜UqKP*gq@ `l*I8 yuRgJc$tM>g24V" A};W f%%ʸm5 bt,g@s?}SITG\H$Jc$qS䥛Lm֧6$|RW`.a7Hs o!POAlI$q's$QpΒ{N/m?5v%&z.* 'ІhAixQ$btkOl*Iଢ଼A fKW˝OTk#HHA?tJ UMbiG@ FHM$h-DIl ;ᒩ| qfȢ\5[$ $a**T0bd^ݚ !I I<ɦZH\1c1dȢtIJDʐ+er@0P'`G8$$^U%LmIRH$nEh_j+ I%qYL\{Ð H6RbH]8nkIy)^rB9^Q ܦWr7ʇQ&*K6붧WH 6yc. @X k` ߠnq I\-IM0<}]tiVqIBFhX8_,?R6Z ]DA(fmV$rZϨmc:v$$ziUR6{2A\,7ڝnuH%+-omq,lY8$Cw :fn$ !L̞AoJ)/Y8@xdh5]fɲpRZWh3&D.s@,\SG8<GgR[);b]b]~-֭&M-$a*S8CiI*'ѽb<Ʈ,I\n/VJX$a! Z>6J=5v?(;Y~ۂ"V/% .hTϴK4 I 7km gxxA$"<>K`d95ulfUST+EX)c]3pۜ B$V(wk@{)ZdvXB7fM$$L*G01+mnsc7+'GΊwVYҵB ؄w?c  GLT*uI||܆;q~*Dvi\|!, ܷWNo вoiB;_KBrCMDzsq?sVDvG? ,YCxT=$f{?Pfَݯ+H㲐P$ %9\b&\O.^'L.a?Hy9J+Hy4Wf^ 6S:sawNtv$\TFvՕL( bI«&LLqyp#;ʔkx,Z-LG8w|`{dc4>`CI.v~ٍx"MzK]K6>`m^x.m󆿼lO1Bp,KsO$ VfGfLGLw[.t:Uab0osU %Ić56!?&&b'M`ړ6kA2 LXv *A@wK%IL@&z.Z8@P w=b?Ӈ)@`Keb_^n ImFhh,4(~>_& `ذ+%!0cVGhq4G kn%1B$ 0a41.)X?A@I;eX&K%Iᨻ-sDyF pjcd  `^M nA%I@i$L̔-| @`Z6u8>IrȐ$$9)ӆ @Ih6 ȀPDvl#I<"{{N|pH"@I@@@$!.)z[4OhC6\ @@Dv &A`?KI`nt0\0QlA 1b r!B.5l!IOPʼn1#@DH INf8DZY R8BJ8DZR6e8@1.(FD!qA1  N"C1 $o>8 PD"lL Жmb]§IgH2Nq9 @ -.$E?0HpvsNW. `^JI gK%"j. @@!/z Itr8@RaH" 3EbƬm}mX0!LW/t#`˨lxq[DKU|_>/e$S^rJwL`$y.%- :j|_3恀CnQ%Fasn/O/SY` "QRF1|lF`1 $ڡ]B*IU jgމ Q $TYZ)qf "M8 oȐ$zr Kj-||VD')A?a'Av  Lq[KzD֯9џ9+B@Q%pCLv 8^XC@hn\e[?^J>T{p@%vpZ䪁)`ikz>  ya%gU0Rt]|pʚY<TRF_ۭ2ẑ%Yz>J,PݙG0 wI^_>s?{q/{ N T Yu,\YӒDp ʧsYCI͹~"DV0{)Jc<% &P%D><9O WB7IoT Kݑ7垩'>ƒX.1xIDZ t.GO*$!Fj]B$v nowxpf-}$ @xI/ I"]bjBIB$ u`$qPH%$TĎ~|}ۇa,C &@zH.t=U‚Kqw>q:z$/JY@IuY,uI@:"#?> oOy @D ܍g7B֦nP$k!.H$?&֪[벚[Y% BRp;+z$$kM L4ŋ ,&I|M.D)^L$r6qD.1ƯA$H@W.=$qPHwY]D[~NOB 8p1W%uԥ]dJYDOWZ?^JA  !ԥpj#\ot9C@Nu^K Y#܇&fשKb ?iI"8j1w:$;e1]H%#ri3'%zf,v9#KtXj kc s"붾s#OA&%lx{fݫwlv $tcAJ  "r J LP!@L0~IMvY mab N~Dy381!ӷ8ܚ$fw#@n'^ Xn@U/w[0eG&0D@: Nm̔~| ` ,āv $ ӯY&F@]bvY%$VFqcdjZ$]O"LLAS)҄pv $QϺGbn,Ƈp ЙItrpZFc v>Ց$,d> HP EsZ$ݮ8d!$Ww[>3D@.A@;/$(H8a41՗--xsD ؄"@\ۦ6NslDx-ZJ$gq|"ET,K IP "*&!@qP@,L;/;_{gD:IENDB`PK !&66ppt/media/image4.pngPNG  IHDR&>sRGB pHYs  ~IDATx^i~]R˷փ &Ib% IY&B&Z ( d&\\Z{]LH2N jEBb)-IfL÷v7od{E}s{㮻ybǎEsOUw8ǙU`dI}a40b ?iM -JH(@!P@!pH;2|]>9kxVm8jv_[WӿؘglaUbzwfaSigfDx{yJU!^ HPtx^P-cuv63!>blҟFVp_`vMC֯+@Uhx%9( ঍g4~5sWq`z8 f%S@!P#V;|=4f5] Ƭq a%woUץ3{zJʆ&y\G f)d0YRdzКBX'csɜSA;7d8n_Kkd \5T˜_{ YONJ[O+~P:%_k[@!PB4]H(3@ sy p.KQ8ԟ^@#9ITê]ܿ4`6"A[`xcC3H!@!P@!PlC!e;EfqQ ךw\{TMqclH%#E eDC,F]CH`!Nm;2 M\%E B( B%lyΥawx=ΎX떴w1ϵv;hzDڛػHס%|/!w8 ;?`7dX +dzQ{Qi+ B( Y]$TR`2ڑLਣs-E!%"l=jnʢ,&ÐlBiZp "f:Ll]<EI (L!P@!Pޚ6j* qU x@\͊:x͘񆾜0qS͓>zps B( B`^ٍ1FDbx{8P$7jVe3\i/zn V<3x%]);F(rM] + h+a'P=J{}t8 K^:i\X |DۋU8p@!P@!P4f(rf4#amtSͱp0F|a _zԎl0WC ~9J$q) <[c,d#=5>nG\q!ߨ B( B(6"mT@!@]r>jF.jT!P@!P@!P@ 1ҏڸOQ@!P%{67ZuXQD4{'A2nEI B( B( B( B`qc( B( B( B(% B( B( B( B`EI@t@!P@!P@!P@$x!P@!P@!P@!P@((B( B( B( B"PD/ B( B( B( %Q@!PSeSxi썩3ɡ܌8Z?nmU.m6V.c]ô18JEպ킛shK" Cq-lQv\cEkjZ5O*+J"X@!Pxro6[#Kgv.Ф3f|aƌ"`` !R3f@a v7OSoKǥ+0Vw\Q@!P@!p yPP?T2|&5Y N` U… D/׶IEI( B(!0-2YM? Xc~kٲ~ȈjfWD]?2~'L!B)1E 9SZ:$>燙6s65\58iaMA l^#P03uhuO B( B؈ԯ#z) ̌6 >ny)zdL=|kf y&|B&::ӌQpSBoO %Ht^wpᄑ6)#[~cEhw +ܐ{ONlp+}4p.D^S"l_{%}nT[@!P"`D8n}AᯠFk h~NWE!gPRkc[f3dQz MdB(ۍ<ǽkft3Ocy7vxcC. B( F@kR4AQ+{L6vnBk9N2P>lG!_.b'8o(CD2/ީmC!k|ȼS[c?LQ+B( B؉Fcdn]Tү|֫c V 4nMWS{qCy @7i3WtѸQw"o7dX +yػJ[!P@!PA`xCiTq{=GH{P(AX[yPIKC?*CYL*wf"W mbMЂ xXA@!P-8<6jxG 7cnԖXl_7儩8n0S_3%P@!PAnO4-wQbX3rFFׇͬ]9萌7ɄY1R[o(\@Rt%/nF覮47cnO#溓At8 wvI) >̟]4C@AK( B(E`Vb5mPS5א

%$C}٘-EI᫑@!P@!PcDZ>] .fTk\2( B( B(EI4}@ÐzA梉`PG ˩B( B&/ˇm1{ڛ73ؐ4(j%ZQOiLId`WDG0"5%A|. wYCDbS.'$J'\S@!P"0rž_'X00lׯkW͐wf4#t;k"½ +YW9I$VWc ZQƇ.VbFI؅ +anG؞=7B?J\R^AؖB( Bxr\Tל BIh{aLo WsDu]c Qč\֨Da Y{)_ B( B(4Et~ö.4e?i6bE,wd48[$b%6{+ B( BF qgI)7ϋ$n2f,3=F4mF?BlNOġ%+qQ@!P@! X>G0"%u4ƨo͢R 򵦜!0bZ|B( B(Qq(-o acpLwvCmL2f;󭡳(@!p4}k{|פoen(@Hm@O]/`wwm `Lʧ]2zxtmWߗuaX B( B( ꒨%#<x,K4)#DnzDbR;.|$>3J'71 oTQESԡD֐B( B( B(J6p׍fR0\.5RDĭġcꡙ}S[7}& ,v`U@!P@!Pb7X4Gh &nܶ9_z ,( B( BSx; ܽ͛5drΥ{efa:%.m>b}׆=2JI\+ѕ˞)$|tZzN<\@!P@!.Gs?F;\߾#+Al;ƀ#xFIW؅-r%D#5oe\$q%Y@!P@!P<&xLѼ3_V.5#RCo#$zBBd |ĭ@^W6q[6+Ģ15( B( B(ƍh5$oM|@&Cd4/ލkC,vjlfn$Z$DbmbG{76żt{7qԮeF@!P@!P<x2=%v;oEIa %aG 5f ;t7<#/ذ;$%Ae% B( B(2EI<%?4WCS I%" 7J5[<vFA9=[$-3. p#+QDhp!P@!PF`춫Nv6&6%Q*%v؟lpÈ|Č}XՇYϿZeϨ'>q5|N3Jb#+Qr%V@!P@!#ЗDه&EI\0$H>b/%hX,כh0f7) Q[X z9?e_@!P@!P>%$Bč`j>C5cZ6[o~QI".hpqDg_A #38B( B( _2Ҽ cYƩc`R8!aj0RFG.M-WڢeHeu%C]2"+Q-+ @!P@!P\ /hBbA93f7 ("зK؍9>ihh[np\Ui=8GHb%pJ( B( Bе} A¡!484-2Pqld\g ىCJbx.q86pŐ]% 2 p~?2G ]b9 D] [@!P@!P{.wܩN _qojKw9@F YYP4D[<(bj2yo)1 A +akҏ}ײEg)) B( B&.qrDDKCf꒸f*MFTb%rז%ѽ!.$x# xx]U%$CI|[G:;QJe%{g4 !Tv?SvF^@!P@!pCS;̢$n$sjJawJ[(?5 V@^g׽5l>b˧KkIwc/u!#t#M:,LL$g$Ы!@!P@!P#pJ¥z$m.V"%!J!LDCA"0usl$  1sDI[$-:1MGaBQ勘HR@!PX$ %1d@DIQO3soX$8J],Wc%v5J ]Ebg&.BI;Jh>bEhy Ms>3.5G!P@!P#$3yQJB+?ٜjaW==4! =#B Jյ_^~^im٥'[!{}DF4|MNN8D@4߬^ˈ’) B( B>6#0|W?} ȶQR]JYF ,wp,vIw[T`ݏEb02- 3{p %}u@8 ć<P 79>%j# B( B( %;S(Y6 opE6E`OItcҚClv )YVB:Sy=!d~U D-lCa;|!VͮfȖgpԳ6rQ@!P@!P7G$n1`1oW7>Ď~jvl&J Z5fpʋ&s|cD΀U@!P@!P$n]N=$&fD"AJ ^\;Bks5iѼ8QڌP ֏A6wOKEVvm3( B( Bh8ǩ'&zVBBFQwᐟJd{abI3bL>Yd%ZJ B( B( (J|/7}¥al+ >P2" [ʺCDHs#l}fĊ 7;$&A4+QIԤ@!P@!P[(Jb OZDa{+}|F\Wؚ7jgZ~\.麣|;u#f%|!P@!P@!pH 4+!ܝo{]X f\RrAggT$NJMQ@!P@!pEIŵ։®Yz=4'E 2bߵ ki@!P@!P\$.˰vpz}ir+q&py0`%rzBq(ͅ@!P@!P҃Ɉ~MXFדo!^`ˌ6>Mq#b6I&:&V,gV/+ĐS+AS+[@!P@!`#PdHX|2+%\O8^.ٖǫiJlzKtL]wjV\h"lG~O|~wOL, B( Bn(Jҡ7kʤΕ8؈3`dvVb''-_ȊwLiza cAXT@!P@!P9vƨfXYGzz9ӭ2԰SQgt->tŤ]}f KfJ. B( B(Jl1{vV,M!gHL2V ,/G,qڪ1kcxxkf%v1zᆠ B( B(Rn|h CCr0[EϕnkoB 2- 3.'O93 1ؐiZц< {LR,Rns 9M@Rd B( B(\p!&X'&zJ5ӹ(X)_h1 -%Vג cwݹWZ4-EIoi. B( BI!Puq$zˆEQVɽs%>{ @\34UhNqݒ>R kOo1AADp!P@!P@%qO'i$>cxXpGںxlx-." 2bLH\$g3ͼHb=P'%V@!P@!P $nҼ 7aMDsĒ}`CdZKۢQ!4P8Ei6=͉V e- dD1+,Hw+7)B( B( BF(fv+z-Eu1(+q‹g4fdK08r"B@!P@!P%H&HasK$P;P7JU{Yjda΂oCJ( BEI\-"Gٓhy8NS +QHؖcE #5%  Gaє9$$ADh }~3:b( #Pc0E}CB V@vk%[fBЅ(?nDRݦ*c?eOh#O[2>!JB| $\?%`>¥qMIП+ &. {ꞒIMI>"rDs( k"P7X޻[Gg]ksTsʍQ÷ЖG)ߜw@ pߌbDB|FL_+M -8;8 { ';޲'J,n=Z" rE6{G\: B((Jb?WP(L,;߯3Y%Qy7aF^tMgv?p=!Cr-<JYe((%ȕ%$r:Zⴀ|kO&* BEI\! mpSnK]@ԅ'<$Ap܁z:}׃) C>B&4Aʟ =to\cSEl[\guKC!@ܪ]XhĭdDG ( "P~2л>`Dԝpm04 ZqŤX۪]ċF2ps !0d"f|' !K?v~1LC%`gW!PS@$U3gЫЧܠN\춳vLx@@dPo0!)yim NAhhi 1-jJpiG" s>%l췷Q4I4co.޳-eLXm.~/h?}ukU(쮀=$_|WbKVf, D phyC&:&hlcvcbP_-bWZ$r״uC/(ϭ'~.Gw~+lhuRd⌻-8 ]řg`d dI'J ?^JV~sxEJjFa"-ՙ N~!%A ?h$$솕Rw[( #y'|kB(%n<9*a"P1inbHI d:Ad]m@;ShEk|h]1R&B0GeB6&&l>gK7[$"&#}-w3zEҟU$E jc2oa(WqYմKؔBo̗:Yd=2S, B4,P'P=LFbW5^tzTvp0ݕwD3>^o9!b1]l{ݜ=rϾxDD%2~ga7ߏnvi2r6UѸ@]Ėܳ9or :|Ǒof"3e}%JI!PKb J'bv =NЋHnXzmZuUD[+Ӂ.ktp!$fCņh-D*C#"MwLD`yEB:#2kWiFz%D-9Bэz]b%f;{7Tn@fDOjHخ7n#xȏQdC!h4{QB(.@Q{%ADCJ0M2+Q8[Oȿ l٘Lf gN p``JLr'J4ĄMIpq }@]<) ٬.1DO/n4kB(zؐ! aN[{H(_$h#iBzAi@ ZWyhDcOisTO_G@"f+'8) ra%. KL|ЌC)f\"17qDd Hq;Û>ǥ[}EIoi. BE( "G G.Y։KQˮB8V.%$7@<^(ӤsՕ9b:Lmƞ;9%JAYh[0ز@$fesJ0q DoF[$5c$~vm4l#Gkp+Qt@!pYx˥$-C1=9찑A,Ax"GSZE0 '٧~{TC0<7GHOC]s+m3K<%Ih /XĄ\99!$ˆaV% d!P%G5G->DH?nDu>h҄G)MCBKj!wND7T+dLYC4!J,d3`$ <WrpAڅ$IdFHa/ox#-q"&u%, BjF좬W#>1F4;Bw##QZ)O͆ {7nHIDVBDKGobpi4-SA'(n >`ܸݻݸA3n( *rp3$A4%J]OR.(J"aDsˎ\XOq뇴ob%:RzOV%NDZg%Y%S Y mK|GnsG ϒh,\9WX_( -%ƽDpK) ]QJPBn,B"A LKGY dM404|HA8dL鴼t*)%A0@=/`?u٨4+A NƓ5GS4WgmU@!P@QpHI ZWDԈUwNI%:J[M Ύ$(;*\sMʱlXMhV2qM - 3hY[H54d[x&%"<( H@Q0O$#$+K. $sK @!=1J:9pG̛V`%s\{\=8(!/pCFj<I`VB%fvZ$f$kY KB]+]DA {A 0 d3_H)|e 6+!|~]/3)i{-m@!P<&uJ"Q-M?suCQ_\B mm$Hk+ĮT\A Dy[X q:4\i>!##rxQmɔOrQ%1lp7h- 1c[u}oё`.xؠcP؋ubBF|֐RD4~%_O$[o/\0 H_vٰE5f “|H犛Mt +X5rllɍK)q(A|XR }L잘Ewc!,MMڳG4`R$%V#PĶLHSHȈ'!aWzE4d/+h( *YY^j\rjEZc u_ $} oY\nk gE'#xjXI gkO= auf̩ MC'|G|w¯ȏ%A4+!|D?KLDu/VeR!P\ Ak/.A!aPgN:9{rrZWE#yG|K|M}I#GDa,HE?=j-$&]fˏuκvh)De+G%4O|B򅸆0 y{=@Wh` @!(Jk0`BBW14cΛbf\UwwMܼCEf%ȅEb,pRA, 暉R^[6@KxyA@ZN}LИ~A: zB %"וgV1]DOCDm4 7d%POڸ9B7p>B #@ [LJ6M3r;ڸ:D6N4*9Pr{7ŋlوh-y v*wsHC\CDžg(!f2XF~-Y$,vI!h+qcX B!PREDK^>=Kn\`ڴH.GY 8Bxb ǫE"Z2'R~ǽ, 4$zuS/}u 5NMry$=2&vQYz^Q򮟥`Ԣb(h%_Oڸ9` 14.Ԇvث͝HؓqV _džP[9VoJ!zTF CCVIڣ-M`:TjwsH;.(6~B#64@<)h `-HY + B؅@Q YKAљa/:q+#)(rD$AL 4kEFIec%|Iu]X n@061mt cƞ&@m(cꆛpUGSp~+W\@!P"\Oϭڲ"T%7Ac b'Ւ]N|D<8ocغGe%EcSF;*ͣ7rehtU!gS3QD&C.lq_LܩaҋB45Ϟʡ#=&d+>VtT^Pg܍ܽ QzexNĐC1ldwz! q7qVr `xgtz`Xl7>b%X+?J/CbBXŜ|ġq(RR~J4;!PDZADzb\ ϋKjgP|C߀ $huVySSD( >{clQLXbiVbņX *D4@(=NiF'V"JID=1DIOkګpXspvg Q\O-{JBBF ,bn6Qc\$#1t+Z 6\x(Ӕ͸JTu%P| 8Jj`q!f҄=q'/D4%dSw!+&t@ @(._ =!zdo?I!kߟhR<GdX< ^@h8QE^ 'I!AF"!#F364>iVZ$V$5(O.b. ,@1w%iH>t?m qͽ [ GA@1¤Y + 6ћcY Lh.J4.V _(VCru)J0aJݛAs(ⱅ) y[&)6!U JQ>B1$h`Pp!P"GITbBx:]! jj@!xGd%vՇFYi.-?5JѦ}nh ]b JA"+$ LIlpX ;J\oʍXIȱ &Oլ?#>3ɽfRQ]C#D ª="!DH)Q@!P԰H|MD4зHKhWɢ'g\c =p(ב8'G͗f%8CB.;!,Le}\4m%@eD7)ġQu.5O~񑍡o!HI]f7aq[)ᓔ8tơrj Z$\( B\DßpΘp!C ==6Jl--·꒸S17nf%. $$2~bhSWC(WB$&B]b~F=,RxCDA8+żE; 2جĬŹvXrw& d1:Qf+q6#r%zl EnBj9ÙY3#^qH/ (Vb1kx!P\EIPE-dUh`  VX싍iJBWjD.'= q) !.Px $I!3J2B)VbP>^ڬ)& J"ÏWRl~MC4+-+/DuBsрB3>Xn գ(4P&Uؼs"sWuԤ~pG@  1DDl,R}s^MWX (V+ +#)J@_"DJ%^e"X>L#k/ƍf:cǐ- RȊrdDnad8 i"AI@.W|HtWdwП'`ג( i2_lH ơschn Vb̐psm ?~w󟣟`Pb(h%_Acɖе :AnFj'.Cg?ytB) ^17+&/r_" (>bWR $k>!7k],|Q,OC#]8mvMbl,[gRh,n+ !Uw#f,Gh&P|(Gȹn_`2bXEKG2bDdץb!%4z=u[avj`V &HkS" |r+Q|, ")Q8tXxĚIO0_>Br[O I-VlVtb Ctu@ibJDOLۣ"dĐ %:^y[\J6$:q2qa{Ӈ3}J0*aנrWۿS]( BPk qedQ#| :Bb%aGou \HVkikYĊyf-(#ok?_ceZ$ BTCh7nycuWO>۶%^\Żp+>BE-uXhn/fv]ІKF`O%JFu|naoEHq! `la#+8fpjuhVz%F B%,@6+^ i!%0=l lH}Y/NdSGcVb/11d(4=vQ/> a"r`{Hĥ8Q5Hr)ln{2g"ȼ#E5l-*k0t$ֲrNڜYi/_BEIs׽{]Q',\CQGHs42b#JloA@1[C>"qnGpl床U7֐ b" #\oQ"rz6I23a;=n6!%P B%,7d%v$n 7:|;hd!0X l9~v#lhңW K!#Y0?3bX㸝,)* $ѩG0 u2tQV O}!PK{KP+weNn4#VW1*_WZkl>t@>Gm'Lڻ6KTE~M Wq9iY|%HCks ٞ|TɊkr%] Zmg6DmXI[WF1 HopU$1*fpTT(߈ƫF. AĆ?ꇀ$q&18ImUq89J"ڍ?N p5^A[]gHac!Fme[gÇ)Mpg^2_5VGZ->b*.U@!p5G~Y ,\&+؋gH&Hm$ ɈB[$h:8`}Y:cgP< |~_2(YIy'5۬"D}ME*Ȃ.J"G¡y~^$WZ$~a*Bkjjn?u7/p\x0@A17*\eߧb.VGK  1M@׌=h9xf^I<}SeiHɰUrM|+@rϘӐ8! Enq ^a Aa5o<\רB o(΃%L qè($ Z/q#L J So 0hA""DMl'H{ p!GT)qenBUqLF!_xTcېU)@>FQO8;VSc-X#d2y /z].@!pe%!jnB)f+ī1K\k׍PKc!"&%uHnacnJOj[[( a%@Jb; x$d}m~g}{~[e7ʤjw nhP'*F4=Ao!mfd6FU;D.`p3m^x !% I )]/DjH!PlB.) 3nB6)H0T/E aXA%"֡DHdJe$ ) -ud8@I0(CiL g%&b֧ƯfMC=[hܝ|in@UC2D>Ğ}]$txSb$\ܶJDuC]@2:R1޽EŪ B!p/\U7!vD<$QPJ1Dݱ8%D-XbyN.(|%AQ|Ƅ4IC+pKnCIFnR J%psk~CdDk$}o>d]9Qnuj0BLcg٬8\.b;JdD.(a]+Pc B`;#yp O% s1DAid.DƵpIg2*_1,1hz Ǘpaː!iSAR %B˕4L#IC A$ޕ4E|5| $؃jbTJ|D(f 6 m;$f];hq`f&r_4 'sEM\BgE{%V Q}f} :pY]JBlaj mQp\1u1d.ay/D09|]e5,F ܦnHDS!$*s=rfGһ\=iF `eI"'6JЋ_h? =_c_<ךj`I %RvFf B.mkuU BD(E>B4! s cTk!ph@PXf%]KS[GԐ$HUw'hP4 ׇ)ܱlJC^H3J(7q%*\^`.݂٘%#(di>n \p(A&jdLk |DCCx6fq92b#+1$n@V%#3c%GWFX(b%_[[ܖfƻ9+)Ѱ#43SR QpHu nNר7/ ״͵+_gDu\OO5 Mo=c]Mcf0ˀ/liv:TK)y~"#l\9.d|FAi0~Gt/ B`{ꒈHz' 'npp3ؒ>4,wg; 您; HsOHƐm[r,4.4@#-X8KB.3) \ ͡F>%։4JvQeh&bBFP>Fc4 㰮a2{ iY~x|le4L6x}z}+tF,;hعQG][ %1Ob@o4dDF }_g%*Lq äuHA]%2{j^u-JdnLoHgRl/HI7q%UP?lS7GWw*ۼrwq_2mK!!L-ЩoЋZy"Yf"OC&\@A׬DЀ5;(_*BXGn( aX60jA 7,!!a 5Ԃ|8+!m遮!) ^ 9rJL?B"J@Ih(JpjgykkP EY[.h( D\[J"1d"0s;{%d0oAdǿc?bK{%E(`Y -+< (rA /VHyo/6e|biqٞh藕!8ػlJ<9&BЉ$[_~Npk̰{˼!I!Bd"2)¡_nh<8\r[$Lr[$X9G4Lă5Cư#n Sp+Q|.J(6"p7[Ry q*4Rp;+6/;| WgMLiOƕp Ì26zk+q%D1xLYqB-_v-w|D!n S$nT@K1l%@D`%ñ?Χ"vf %zJBϥQ_L4P>Iø\#VMK΍W~ÏLwA"3'.8l֠qpl mxhB(z!nbܒ/K>j!&aU3乞u4El'A\\yBڙCd}[a ӒWV#I[biAo hЋ#€t( Q|Oc,l]08L$ /0Gm;ռavzEJY{+!1X &&[$c"h#0vzuq kHO[jAS,dcW=4a!\$$^COD}1 6DiQ,`>b#( JM_:. ##2C܁@61 1pT&}LE _dㅒ_ܻ_ℱ81`KP. =#GI855dwFߺyb@Jb+=ZdpU 3mdAaO~C0laS0J|]ܾQ|9jB0WJ\Za%rG_$9brF"7 ͉!v`y6X@IhVEpfV .~s>AIм3V"DIDb3z}0궩b%:/f>/@;(R,+v9=0~HCf}0c|G F0GiV#t@۸!wp C # :#c&@^c@NؾZEȦ椽_G2 aӄ&L>b;^?T,^#x] 䮾zĆ3BAd| -/Z-S"$=䭨EyܾQ|љS B c$nn'sH tDZQe9%8Bg3 Sg.*#6G#w-"kChX; "YP;#LKWMCQ>BȈJbhumpة+BF54.+Q|ĮYz B`#Pj=Gb6g HMV6C"4sHƍ5kcs‹1]sKȬ,#鴯vs~HfxZ&kݍ{VG M]\X`;289IlwPleC7z /C:4Ye/<链֧jW#gJ%JI!PlGPWsƬK{@+n+fX*i\y{uX:bKڍ2(S炾[)\icv$W~W8VkuD2wzx7G1bGz?<\‡"ӜG%$%#_B63һCVBPrfeiz> DB_z B`;x9a%0 V.pʭWF2=bgCZg`Os P+WJC-qMb4}`2 %GH\є_6O?®0opQPs\=5><vjG5yp==_;s?(S еGU +Q|( "(#Vb{11QnY"dB50M9J>Sͭ+N'ͩGᑍFjs)oʵH`&( $ ʢ?D7#[֍ vb#`Y1jTu0f Ņq("}֦ SX 84k y i #ff +Q|A)Qj B`xRV[PrQmQmV":W2c[.M锓3.:WsP @J|DCF`\.#50˞W|+Z DA|u+=BM~a> iH|9z5+9 > 2׽R B 8)JT~N"\G0aF\awK1k$"퉳qevc!J'#xRnZi>ӝa i RS|lOGx-yD9"34 4%ؼ)7CC}I(Soæͮ@!x- A%G|?=lE&iX$( d:beZ~DČ .Gؠ!l;.vʇR/uH3"KqA$i@oirʃbZ,G46Lq$ e7珽nTn~K_ B )[[Dw2e:BJv6h_NrՑH#9:/f# ? #x{$CG&~?7~ޔ|xf( e-E:5f+za18 (]2pb0.v=W #D:O_X@! $( 9TCdU|B;8P{%z;EФS\Z j2g*ڻj;M$%e@4 s?&V7,3k菐%]c;a?#">[٣yD&z\#`ݨב_ߠ$.Gyb%9eI!P -9%C@GT;M0C,^[) FB3`xݝb2p`@Iq9rSg%xpb %JB79QB3McK:hqwjHp].aDXEw5!aaXziWB^Sx1( QVdSݭ BP>PQ#SyI|T ucE;{mG0groLr.BȀ:ElQ9Q M{:m7#|Gqs剆ou) J!b3B `S4>bh0c_'L} nŪx<㮴X'R~} 8~at}@!C1Sͣ@U {ې:Pܨ#p0֒ڞ(/|s1LH>gCRr  ff%\bl h;#eoG,zq!GʓW A7 J}D !hhG7|A2go} &-~_-zJI!Ph,7JlVL&d!#p% %# [?$L!=7+ӭ H艤]%&h׫x;#}{9J\p,N[l_@n@ؘEF !41KX j`I!h)E IQK ^lQ2G1C6Փ/⡤OHɈ?B%mm4Ž4J1ќj9lKxsY&FEES8kxL Ű9bcN"Ĭ!(kɔBi"p J!NPƨ !#v 4 W L )AVQ7s(|sBZGP i3)dD¶{lH)Cm|ѱf蟲CٺEBdFQq!4oH8̊0G:rV_XH \@!b6K(ыNTD:5(. cEb3?`h;nZ ,j0̆N$bD %'ȜcMIth%4,Fİ%z kV"]uj$]%!͡5%< ]lrW'I&hD`M=-S{:Pt@!pe.JI5wP XAUp-p# Ipң T׌l&>(+9&:`wlrϺP'w X_ck?k~!%峻D_0_}R1 ئqJLѧʮsHWB4OI#Kn&'nA=['Q"bX5iX! |N?9]fLDwVל3ppI]=3܁$J3nXdrYR 7>2G zMLo>tqm4S6g1htsl8nvXK i>f :Fr#})ǯc,Mh2 {f89Ms >SAM)SW`Sݣ]4Ls$GlDr(^5Y-r'5F=u9A.UA]iJBGl(5+ěVfOQ+GJne~ (+^#N؟78)N^5&ٺH!cđ: $%c7qBDhBGWu wGUñQ!> H|O!3һFUudo:Eao]mdFXdarSHz˜lՐ =|pva ׃h?#$V xֶoņ]nѻ+=ܧP !#2h\Җ#$$H(UšYPX@!ߘݢ$@t[n܈H=|]TkwX rjNpL9Pfo'm",3jb"*=ܝT6л#B"E>Bvgb1f \-A_Xfٕ=ލ mho.2!U_rG6dRD&] Gv)h]X(qg+6, ,C|DG"/8$@mUlv _B{3rthBR-ﵧ7~ ;j SumRb0Yߵ0!0 |EӲgLLJp~kO@~va"JXi\nOL.%9F#7uzr9C_@4[gk;)W'qW{4sSO]P<=.JI ",I%#8i_0-!f5ef+D\#QdGT !b{j+4-Qk& /m[MQXM76J<ԙJh2WVn`.fƬg<CxJAV;#M%tp73ܩtj`!Y }L8ts%fK-z< yi{:CÝtص߰Aޚ["XJ!8 pqrd/ฅ,yhy  G`! =ENϛpO`s%BˬJȅ7PQȶz{,U^Z |CBb5h,$zͲ6@#vt !]i!Jx#@@!Pq ʷ|w-2D.=\+=nk.avBa̾|#pDr6mɕv 3 .lh ;`_@)ꛘJՈf  njࢎ۟&]~@|8Cs|N!M gdf { ( n6DSWsD(%\钐o,u׮O  ɼ;|(jx3F]{Wk/xunࢉZqސӱ=A1d^I-owo) ^ zq QMID$ >B '%8bRRꃟi0V. PI.:)hNѠ:ͮDs~XU.oSCsڎKqȝz)5RBp/9#&KDmVyJB߮|FBv]Uz VF!S5X`2ܠus>B#, aۀSN6+aC`%ٴHDIPvh0Ϟ"fƮl2 IhpNCIh:&HhČ!Ca>F3`{#S)}Cw9"/ B`(mQ$ku FFYP`f#NI8g wnDlBka*dz"Ir,F6n#g%veVḢ2wD#xJl?W( EB|O| "BbH! ]uji>Bbq %KJ=͋GD"Z -) nFADX,Tz 5k ⲭwvV̌ ӱhXs>M'.7h;$Z3rJtrm2&1/tk/iF?r aa AcC]QJ;kU=r1$LGZ!/jϕ:Т![HI F $=+4\*B!p{Js wýU5պ-]%!abQV鷆FJ9)4J"CM7 %ҳ{%IO\1IP0yfFxjp#[@!U({ :L!Z5g..145.$%1H.Dqd ܄6_)v<"BȚE\F2MP};f#6j⅐x.-"3Lyqv K;e"Sc?+h#-X#x r_7{.Lmmda)z+ďUZ#I}0%V\xvqBY@RY#Q] 11+?DEN/!q`/ ,n|A=25HO N9.=CȂ]һ6PN2c =BB^ ) aɃ&!4o٩1CCX] 7**H׫GbG+,@cz\:C+jW}E;Oi Z(bEFD+BXArK821uKވȪYa+ILcVbfԒH 6_ 4ZWL]wvn~.qjv14Ɇ48CيFt+xHhS0%qA.]-/kYnHd"J(|Gz5L3UEBC°DqvgYu-Ѣ|!PcQn?%I7O~s9 %VOR{t* +cwzkWE7g%kmCJ%T\Yڼ, .0Z_zṤt" yX&PsDo"cn6HE-_)M4D{ʕ %G[QNct BId=ECO01A/jQ:[ݕQ8:%s$S+ uKIovĖdDր9b#d^pf6b@Wa(B`jP(a4 ^5&@z0RQGbDL+'&f|&‡W,)m]$# i!ZCLiFI4x}N =_$kx tt Y! xOSBL*Hlz&d 92VBXk5s c5햊H Ѿw6ۼ>T&d"d 7MԸ%ECkG]Fl˯-Z_ÖwMgdzbx롐*^2ݩW儉 ~aߠ*++7`'S蝽^pzb%m) LCO"Yb򍪌)譍lWc#%Bpp!PJLx'iP Q3@lhapކ8rk>[F&u}IbVJ3F7 u0;n2LQ4V%WsikNi,x|m+2FVԂcszJB"a> 0J#HgOI܄`pѓ :LD?^%:ȗ7CJndKڐCyd.%X@!PlA(gW%i FpFU178|SJS֢+xMh0=%ѤgQa8$Bh?0fm J8 Ą؀/9ymϫ>9%ws JB+['fS3OaGVKI<1/ve^!Π4!6  lAL+AФ 45.\jkf#6D-(놤Xa(QI) Ex<tC'PiQhXIoJ%8Rb@F\AY7ҏ]Z4k|fDɌAfgbFh=QkѡĄ8x#KXx/7R q EBK M [$B5m-ԀkQ;sj$ 5 $ x{sl%# ZÐh\i(J/\b@!SA+"cDTް!]Ǻ~ yYNhXYjn*U4*@j!t=%~c YCt[3fpnj6N /14\;B6^n[i Q"AI3n$HQP#P rP/ IW$t a%H $&܂U_ P?c(c{aǾEBΪ(Atĥ( VF̛ehX|n~i Jл.1aSla%]O&X!#TWAjmJeo !EIlGc$4(U)M=6nK <1Dvl1~b؏E ˞)+Aeǰe]b ͈ 7ͳPAs54ei4yw"7aXihVB^G YU~Y(anK I+ 9eIXhώئsoԀ Df#6roOzpeX@!?J>޸GK>ZQLt M rLx9;ffZk{U=$xz}d j$NP&CŽF #2Z1Ja!nػvIDAHFiE43%q Reija#2bYᦉ+A^71>$$s_JGf9!xr֐wp(J.7Ohnϕsr"T3"%4Wa5k8@ՐG4֫O(1!WopvQQ&QL+=wFI$vm! \rh'&C"Cear4*WK򐨼F]&r )!bbenv/pI7KIlCcD378Y 6`M n6pW꥞m$W*QĢCu鰀-%.oŝhЛ& ČAȁiL4J437,J2X!(J%!FnoxKѳ;h%\ ,@f@C1pl8 &,rebB)) QpFDSna%R1p]%r kAۚ$|~JH8q&خumS]@Pl`p!(Jbܽ [!(zTJY^af["sVYH]Z$jM"d"9\v2jhYr8 ި Q5<~WDjz~!>+ IjEvWl&])MӍи6+Aþ=٢q_$:κYП$/F rWS@( ?RuhX3bhc;^'Ki7Pr4:GdPZ E1[ $pjK:%!gDQ\5xdKIGI{ 6=%I=n3D촂!&C7v 9`\YX,%C8p8aO\p lCK;V]BpQ a"34o#&&JBH52|H]^y8/&qq;2>/k .n$z>B2z%8[b-E2GGdLl#+5Dpx O) AyvgE9b@/MbR3 2C4[3TO&9m>̈:מQgAl@dk#n Ucn2GDDAN$ W1]  2nn1&Q*IZ#R1qà$\'ճ ߷` `"2P:m$aBI^ # }=D1M0P3*.NZ^"\ܯId Uӻ 2JdhT2G*J!㬄KIkEΆ0hmhE_rĭ$]|JhJXa%/62 y_2"Ȉw!EI@@Jb]o­+&SHY(*6r7j6G1`m%]o4rLap"~4^nX٣Hz'dp""`z';ĵ L>`bpnE[+G$#e!ϜyQ31WHD"h( Rc%\ ڝ4̉Nʻl[?Ͽ_e"#vZz FrĬ>Q7g(^qkW0*Lmhrƨ?CMHEՖ$\a`YB+]V"Sn(ݸ)BCgEqeκY¹&"MnF#\vI( F)WIUc7=\lg6s.k_nNIpbFJ`%ܸ#ESq7{B m= 2B+2"r%_7ǸKf1!Y !#E '%( v6tVVb(nHO Gk-k ]b.pwB¸Qa&0]/3/81LMH:!u3w9,.D%tDJC^N ]bX0 %#b ,gHq7<.=MŎy32( p]bHIJ,|HP#VXBfX"#L!%%zS܄mۛv]Te\4:Ju*Mx\OKE ٶ3YH@ ӁCG<;͐a\H$Z;8]J hPR iÒtyG<<%NI0HDV.Dm$މigĩ(%a##2=k =4d#I8gHIfpVh, ( l ʭ'W)l֓ =bzh/Fjsf0.Y@!l 0f6d%Vt&sf"gB V G yf#uAV5pGl\%AVIɔ_43#qyЯ%YĬĬQBNٞv@[#=~s*Rb";.+QmѯDlJ"w#0LbK. vaԜ8;s$S06DPYE;%✸ nm|.s?;ClpM jHI<((EBG*PhkjZ].e3:cLD3J K@i3:6ؠ$؛x:DI,VډN;+PYRy%e׉w`,8j?T4ף*lwבͼ.ztλ 5F,Ʀ3$%D]vc$a.wG[Ql?^W X dғ) $2M'J,.hFn""3!ng%RR^ GMC3J7,>2Cd%ֶGX?p@!蠇YMg.e(-XĄ; ~VZNj\tPgW&#^ hh{Ȕ}Y`{`u N KdQE$l$†&AI<\̽G|1! ,seX Ϥ$ GrӔr)[dn,MBr"~PnXi( ;BJBn%AU{{DYYt#@7^l ې1H!2 MF4Ą·if AbS^x2ZV%L0$V"d]AAR>3㥴`˛d˯AIkH9âb) 3ى~Lm@1Wě^ˍ+>`Kg!8xvGܴP J‰ 5=AK#[sv1)$NCHCf®} zNa%$LX%JQ6Sսbjug Ytƨ|o&W@lV NH@NO:EI,7C>B9nXHڵ+Xr(g0J%18~qh#Hx^n-I@~ <%Y$.;%Bw~Cun,5[H8=K3c{HBa(%D/6F9aja%fG6Xލ=阆Kw]a7S[x˼FOd;'J GQ^ːhb/.[Ffp~ߕ̗1Z-Lnܐuf7d׵+ p+ 4).H^|h^A61q\LS5D)) Q(J" 5(4vIdlDo7LPhU{(1q\XqY=1[reV"0ٵW><_|Q, p!JO:ĄM$fE)w2 B!#'j> vS%&vv}nb| B4HF|G0A4W66 a.q%g(yDM7t%qڿxGI<'?`⚵Ay1v1̞ DA~^.LmS|W}chkjw>gL t@d\Ky@"i2! ha{&#__3$nBL[(χDs1 ͠Fn?dVboi&4B7&8e@ٔE6. Ul[g(t5,g%P|ĬE@N7Zܒ/4vFLliT{4JX׼#( 0bnRdr·iM=/٣#.akooC7nkcbA/ htZ-uPW8рKjH]Ag]<_#NXquX2mkKD5J, t>4J_tk[eF=:)7@kˣ7sa,P4^Q•W.m 8d1y*g4v{tH [?PD04M|@cL:d(Eb"{#ͷ Ai ղ9"pɣqEH;h:$|.?rT$KHL /94%ɉo"d BXD[,Ģܑd"C1'B65ì$%`$Hȑq21f%>+1 85J }Îc`z ^{tגԶ BtՊ }>chH3ECFk #yw:4=xvt@54J2ȨE;g + /xQ2%A6,Z1C{L@ԯj( ?M$( }. Y$Dsټʼn U&2NI@N &#Y Y.Tdw.RF0KU!p}KIP;7, ݈_S#"E5C1{ҧQ4թT 6屗%Az^ޤ!'*KT5cUͯyEW-|/ɐo>LUO{$ǶP Ľ N?vqFVD.X5S%fHOzPy#Х95b%F/Bepsco MT-"q8uK tK%!S$ ,~#10 %a(d $\_p/4Zm[~M6 LrҢII4%lpQN)HI4_✚< ˍFjh 3 MDJ) 6-#49JkuK%96%o*-L wHl]=tK3kԡ$y; 1{PRh#dž8ӌ{  f âddu0h $rIR ;E(6pPYEGԽ 'bBqОZ}Cs,bV"%Qġ+䅣jp+`З1RTz6gq^Paw Q#ßU-&'Y y(3YCYU&'芳л +E4PDzB(%a?Lx|܄qfϯ{T)dǰWB:|qDI:4"%FiJ X}X$8pt4`(&͞h$’_V,id24^7x-9D4\̑Q. Jb;M7WQOx[}6+qrv4f!'d^W$rdP@Qx{ bJ>>i$N‹nY nY4l-qv@D6l#핧>r=8EĄQzz!tWFFkU_+OCS pY(#t@B Dt,8EIFm7Wa}asx]Y`D2Q0΍ғ$b%cҶ5ElQG oHL ׄ_~GX %HB$u)Pnjh7o/?|0@?~(-$h WJ,s1]tHh""2r>7t%BTAפCwMN ؒ!:Xds+\1vY) #'g(q"qPNmD009b"W>2w#k@t%^coEˣu>NF ه~8="!Pkb"ʚjYC$ɵ6Q”}݊$bqtX.=@QWP $z%+%ɁCq_ČؕMO.g1W[wf7X 2iHGF$N|Oz}FR[%&6usJB׊\R7J\W&VsR 3`Ȉԓ/~H&yk{d6J 'LC 2P BIAFuqYT !p8%a &#kVjG: BYV ݃RhcrHP`m8!~j`ܷ.hiz+wa82n(si$nՏEu'D+[W2#R_2">40#dݡ;PBE8*C2q%I%ػ\p^0WY,_ ^yq^E`'%1dgo+6~D Xx) c]R!ȭޅEV"TꇊYZAl]Q.=Cv ob=A6{攄 LW}TPHы/DیN!ȅwB,:w^ d(] dv@!P\mc;mP7d(.M s`{b1g|a Ÿ}拌6=6q ϕ]TFA;EIxɃvmquD h*:}[$u8KIh9tDE쬌$3\~o4 |ou@_|$Wk!Br_ j_7J!+Qʹhؘ \dG\Vht RG9|s CW@!pYnLIPo+A|d Ε C!CD \zo2'bJB?oz&%+Aã%،10Jm@H7IJ`;X[QbAM.( 1;WOn+[=iHCTC%$#B롷Vʹ&g% іF zg%)ph{wSLQ0xآ!\JI!%%a;nlv3v2CXRFk ѤMn!#ץi ~g(1HQ[T -I2qҐ ׿-<n+b\_E%AFs$/Z:!R// ]Ep-JXa #hK$ŽD49Sɱ!x5=KC% EC@!P܆pGwVnusae^a8jνSw,,Z4M43 EμfĚ58UkN`xQ~7I.{1xF nQBhI &{$įG*Ƅ!B*vAhY mL# (&Q8{MZ[ɴ5dW.!D7s ms%hN] ri+ "p6%ރn<SG3'sMl#fF%!#8g-}MZfţWk`g%8r8[Cʣ AIʙ3/#Os2Z0=7M䪸Oů{+n]7izBQX& a(^zeGh"c#̵:,k b Q"wcy.\+ rٸ@8V%Y" 1td;,d'xHZ`HI޾$# z@fm} {o1B "p%\(؊#;¤n}㸉!.$̞ް]d%z>bDoaͯh;{/$gf 5CҺY@J{7@bw5H^m}DjKT%!'PKsl8: 1D >=R#5s( f)IGٕ זؼDQ.B`$Bd۔|A=P$BlGJ'w$ֶaQJ3~Ύ-`9ɈN?~|///[?%CטД6J (2{iZfihRD.7bb.%! "#X`%%'R")!_6!>H@"nv5A8ؙM"&^`ϻ@!Pj?选Q7O!R Ʀ!  W~(LrGh2d>~JY # W4F |G( NĪ.aTx 4JllU>.Y}2$XC z%Z9?zJZoIu ӯzG򮟥g$'=nf6 X) ׸W $@zN# ryPӍZ$O=]=rO|#gT WunB`Ms/MI0=Cd>~o( zm#'Qo|MJ#) (EO( 0D*"Gh ӻQJA]g$C( 9k~DW0b&p4' 7Oڦ$)x?m &K&ŴFOC(Q=ج?c+ElqC>bX3^[$~/D+?{Jf7 .$/ѐ %̱ %Ax%`( v8Eqk%2^}flJ8D C1IMuWY95}?{c b 7hmh[$8!.>N%B,+w9MEI@!P4$>p7w8+G,s+tJ-~*͢Y `JN$=7d%v5J]Ζ%즐NyÛiޟ~wC@vu++}}ç(E`~xZJ\O]*Pt(g׬$sڇrg A5z<5s%J)&XmfR #JIa) jM%Z%\!ijk_o+w[0^9VduI%J49!0SʐДP]qCO] >l33 $I]I7] ,PT/6#&&h BDjaQ$hFJ<cL{vK39@(o{^l GI.%+CBcbiJϯ Kx1V1G7t%ðFO)g9{ǧ{_ %8f-)he% 6+!IQ kE|_i(kRV.jqCKy  늢$JcC`kPO: *__&nEI3Vb9% 4_AKboy#9(vI4HMETkx!P)S&ܒx=٩7N6!%ͱ(%pG#])ViWa%\-hAI]u~/} Sn1k!@ }=N>m"AINix ]b Wgn"ML\"썔DT=GAFf*1Q "rz 溶,sEKÌ\n( a.P,7q$%Ū!%[ Ȃ( km!7i`z=*YlPO F%,&D#7q 6XX&۟P3Τ$IW,AI 7!|DBCO܊\s-B7$"xt2dord7G%A6jfuI=(A[]C>}o>7q ) (DQ"f2X$V7$Fv8拨 ؂a1,QAYyjk-λ%3% Ǡu\g4p@JBDPlsmMg$}IG@t/JIhywlubG4=QTS"oaAwAFLmJbmHI|M--bJ( a%o=DIWDn]?Q COt;~mLSB1%M]k {H`RҎe'Zpt,N|Z'8B*C.'MDzΓǂ'[LR${@%-]=%#XҦ$))%Ab|;"VN3H 1D^&}i8..ލ_bn0,R=}_ZgxF(eh-9xݩmqV( 6oAl(Pgz^Ch]$?d%N蒘dЋ_։ $Kx\{"G(|h%A&Zmjhx#R=AS RάMI@JpD[JR%}]A+MILLא) 9BPMs:ځ  8aNIMpXo~@N:L$ިuf( Mju>BOWbF@QwA><ɕׁ`ؕ0+D+4Ɔ( UJgÎ_'G?_Gd.$f\+J:+4d /V=%!  YrC-O(G '=-Aޥ) -3{.oa.;06[Re" $}#bPHiHb#thrD`):ܵaϒ[$jm7 APm]? )yE'P4UwF>M|nhwrmy:pӒ 8]C"J܉*uD:Ij`!pG~8HL&]0GYe$t|J,PDQmr,Wzwo[}q)&  `it[D_%_S2+_M9KbBKP 'nP6$fKRנ}AXO Ni k\!%wvZa%И4uݒ0tCrĠ$dxavJ)eC2NIpD}_B#A)`INI}C]Z<]]@w% za% J!#xv ޵ѷHkpK5MI-̳$t^9o|긋UpąItYM|eb"TPihҵ]_,+:@*E'JI&E2[$\c))+PFCDxqX>+ZWJIpK?W%zJ6no魿=t!!!vYI"[n1A_$tYO-l?.E DIrΙ1cB.鱷o<. .1f=hNI)։. k w !h@6ncq}@X 'Q?Xc 6J!-*PJeO3y{1a4ʕP%c4J!.1a6J]-{X ]d}HIhw7t-&%s(X" WX `0XGOI0LAD$`7;NI!7q+JB8Y >Ky/Mvc˳9)EI[b@! $?V>>]"&= jETd46*ۨBJF V¥$`İEqc#ÖK~hƐhүOFI4=}yޯ+x-M >0%A kuQn>w NW-6GFƀf[MnA-J#G'U.' .Jb$raQSFK%7Z+{w 29`8& F$]:~oya#>ub=?_|rxa2q|JqB۔wpD'{w G? TոM%nRG%pH[ #( 1jβan6mDD(DאB#p6%,"2jx;c OW"dA7J+çl\Dw51 _nrM #t}C~NYz[qkYúx/zm=HL%Bc@h!C?0qbhPlFK:βaHBd+,Wgm|Q[{))R"(Fqj%I~ 5J 4F9i3Beb_g绾O!5ɔ۟bR. JBtm`r~L #e]-T,|9kxIjxICF&d/DzG AfG( 1j3HImMX؞祰xGI෉T >]$MMGliJ4oְzB>B+3tIk#Ja=+\C2!8upv/KJ$Z$(MD@( 9d%%ѷV"YY a8Q$د[ bDB:#HϬ9N$KS&#$ؕ&4}ARμJ sA QD5V7ąYE8(D(/dq'%fvq7͈4zFFIM6MTcTsyLWg["W) sM0$å{魧pS~y%Y6K%! gM +90:#Hb%׿yoB%!n&zʡGOU)OٸdD( 2,-эXم^xTl6m^g%rMO캅{ =Gt#Rn8 7o:+ӥ3}a"p BG. ЈǺoJ3"/[IYEwLLOLhJB֫=PV3`C7V@֋WoMW?_M4ш.hI#1 gv? ߁H, JBE%"~LW J&VJ&Y>4pk3ߦɈ].ze[S-jmXA3W7'O0( # (Cm%̓ hHZ=QE軄>J0ѓ$|Je8[q.l7$e1:v6I ̲|K$!%ѴEh86J-ġzΖ딄!MFhtUO~8=?V+Rlpmi {$$+j%9JB_ͳσCJ( >u/%AVDOT)@@f,a߁Gm{4+bhg@A "pAHIݾh#=z:IZ.; %ќ!cuy02膨bVbK ʳan~)Da Ib(#-C(P$f|!{7nq=z7,$Hl'&`Ul1CL!LI^+r] ?dt)YŸ=f GMH hXohSJb#!K!r5JB>hnukcvw{Jَ1%t~(iƞIID3serl;];4|)q(CJѤJYJ|!o<6za6"&8jYsw6JمRԼrPl ޶5]řƻd̐t# If nB\@nUC4; 68;ȌhP '%Au'l0q6MC/v.1Da(ҬϾ{`HD%S'ݕ+!1DOIELJxl$49R;#mQv'=[?6`cnYWO( Dfc^hT ^66V˙rFi]6QٸqGK^87mUyQVB3;"JZedMؔN-`pS|hBibbG"&MR^;R/vOE]:F5u\![\cװ*C6JI#42ܦXFq9x y&nh `bQ8z׼I~Ϗ% ċƤ@fbQ[m8%1lY;J@$m󶇛O2|Wbı(z`Y_wGI/FDj@:&\>B\+b"jH!8P".0cCoH~%ZCI0ȋJApC3Xr2@E5Jb=RTŹ% zgP@-8 N)f7bo}; )Q}5KJfb8 tw1ox凬 J?&&f%ӹȯ861%!/t lgq%ims=c(=@KL'Vh`,jo-Ffz4FQ> eQsFIлLI-40$Bڹ7!4;l`@u]]?)J­uV ]1|6,Jh_gv;wj9~m%p!x_r8b(pme=NR. n#+ML( &K\7$^?FwvUz"+1lXssЬWkDQf|}rUq E;D~%i{d(f&׈D6#Cr,â y+qw]Q!50d+  a10zb~Fܿ@j/5сWDfct$:41+|^+1$^Ezz- \G(]C( tP$M C@cl1FC_YC_~h %1cH$N0 7~EM;wP%31A_ 7PxK''D.Bf,bbq]B.%,[o]gx0 KIDсQyA[@!nT!mmve%%!8\cwn#y`ӄKIq dJ\ȱw(^UH*u2Q"UưmR#qd;HP1vDCI愅mI~!&b|^C GE)m=4{nyyq +ĖmXHY^ 0M,,q+om>gLZD{eT_1 4 =%M6%V=bȐBI+qqJb{1blxaM Vv$/{uJ ؠȄ-=.%1#Dk4ϼ[O!8PV2J<@ēMr&ᶁ }ͬ Q-%q{!Q-[+?+ت_GPJ.coP=k0( ;tz,%qP506eX<#h0b@"@&BfR{-힏Gu$7< OrDs?wE$JIHtDJYl'zLD.HӹyMFAEry =+v>G%pJ!7 099Tӣ؞0 MEIlIRR t/6v-,=hvYN3fwW8kο!cB{uAm39o dCcl٘ƈk8H ] c(dfNHQh'f!LLȟj h8c%Y >n3Kf(4ji" 1y 6D4sm!& %_wS9\X}c]]YYz6BT풑OugD&]h=_CGHz$Tdއ- g!CN+Ӡ5Xv CѓC ӻ6;xx ͅ$(]\ps#)pB܊P^_֐hhfj ayv--LD&&i"R<5{Mz("61U>l@zXf&# #ClDYF٠CE>9"#|-f^"+}#UFJ S|%p à$ ]FܨH-bzQ:+-zLϵ&(fb f^a &X_Y딄6]H1[f# 7I[$1uD(7Jx&LX4հЎcB(|5 T2Fi#r$X NL%^Kx4Y>P+1P3n݊ҍ'SjAd\&BqQ'kE~`z W94pGM#shbR_,,ZDaiJu" -JL+ Ec׏ʃf\YlVk)pܹ@D9J$i~ϟQ4u%ET_80`lµ MI LA(7tfng4M%FIoIwr|3Gw~>mQU3\4j4?Xw>RZ#t h! S6b/2,)N.J"H_ҁ{/vS:'|i{7AS-$II]QuwFKVN:5ʣ< |H؅o9Rf!˷_H8dp,-$̞QHYC {GZ]H|{U㟸~৩zVgg65{K㌀2cmΈxz;{i޲կ]IV"!x ]BS)&>vaQ FnJwcƉ Go o9Povl3 \0l8EO&C@e $^@  0ڻtIIn$ց- @!pBI0o7DYXr%a!Vt歮Bݹ f4Oa(+ns1E^D$8B쒢/>Zƚtp\8b@KXP 11#xdsPؔ 4X( Dxns /q%5n^G H |CJ1rvYq/ | pWB[.DŊ"V#pG[mMZ{| _]MbȽ[xߋ#Q;WyFs` ۅ{4A:E&~ޯޘ:,k!rl^ڴ718ٵ> }h࠷e)̕V"B3Gf I[> D77K3*D nx~q}@K Md#w?y>-F"p. ٣a#5햾"7y݆ecvŀ2 Dl~)u6={4z_ra@ Xfo6[k}hHI_R m@ <=@dSv h6q4,*f{%Ba]ohtzldH|н|wu=#h%q+JB[~tDŽ !^=+pCO4Wd?OP 5B^үkhI3{maEB7  Is(ZmOFGcW4L8/ /yF?Pz+O f+Gb-%^DL^ |񆈉(%A1X$H-RWMI]ۙ^nQfB&%)<$Y $H0WfeA`ޜS]wf$`BN$ 2bV7}]xFcB' ZzhQKQ8ȬY䱺RB[ E2k8>BB0P>nYvj9*w|Ʉ$HCv!f31?9ԔXchJc%6=1 n2J|H+< ѯJ%s\t\9,R KCLTD.[jT!P'Q6A$W7nԆXOt9Z %!itL68 R{4VzUΔ9g.[Vex.5g2մ\6/3bbS>s( . deF: & /.%ԓy ,CHYGK~G׊? ,Fs ;8]HU9ο|'=LZ7pX!PL8RevSOxɱ 8mY@u?Q3E Q\Y$E?mSo P,^T@ 1 k>f%B vI|J(DZKt1&ڵNX w@I[8OF~2L'.`_JM¤֍.R-"&o;6g2X<Z֣| Gi#!|ȨLRdf%F :!Yw#l h>X臻 %s'!l|=F,s݄Pc'~H#)s nCH+ C(!(J⒮Cmb9˃ JVY5kKNp 4qEoӾg|;R'#idf2.L]3g/CFD )oz/@Pu^Ȑ) ^!p;,숬yZ"%/B]) !GpPV^ 8p<0ke*!V[&@!@!#p3J"t^ s6zY˞kXAFL@ZkԖCnż~,+;nG0.(\wBgCqj1&BHEnB"$eR] /5wikٌC]^ {]7YJbGh22aV7QA-@>7EOkRD/ .%vD&2*mJຆ-EO*uvl[ҍ>EͼO-܆$ FHzz>-ׅJA?Sbdx9ÚQ'P !ah4%!CMB?#ֺyrBML:&L 1 B` $w!ʋ˺E0EklWa_ُ 5F2sԠ$hB a >5G'sNu(hsf=81"xهMf\@Yh("rg_\G ( dg ENzjܔv8z>i%1Gp7Of&SjӟD7A6'BT@!8H>wn:S _6y]gz+ φ`<6a};<:WfZkci6sfL0+(<.{B{CE}mV5OhӔ5NWD0nz,V) @4$Đy95|9V`&8|&!ᚕ3Nb%3O([c{ClgbH%!5an8Bk$DI0*6D QP nmScQn[(Cs(Hn4ޖ`rN(AC|27s a5zޯ:Q8җXv8&F,p%bLiT+e[կEJQiXyBԭ&| F-8Ol`~ks]aXqnp6|1%։jؕKx"AIgoQrhPAnw[ʭ :i\T[q?!z9rmopzd8|\ QMf"ɫ`Ģ@ȼN+]52jvXq$f{4SR/pkDvG+v]cYj:( @pYt!`>h mi>-q4A"b;҃ohvtB "p71"tبW ͡n^q "`e0nck[=dHN MY  ^H\PlLdswmMMwj2̫\F,$$XT(50c%.(M#fhV-gf{$gUe>bA‹|ڪh ff(&HJ ,R>J#67%R`FI] Ajœa&s6$v.8^$hȂծXi,G.3"^qDweȺ4=U U9 2ʦ$p2;GJp3 !#!ዔ44>F:ݣ#{Q=1ؠT%&h6qDs 'Dk1q+b$j#sd\Ubq5؊}p s͒$:V]|^{S& \5X1KSBQG^RK;ȜDL\@E'J.xygtY`H^&|D$F(=A]_䶅qאIAm[|tAKB ^Uff,h(VI),7$ly񵥒m]-B\x!*! k^ҟJ@!bF)<54LDo^> l _+yyBiW}yȻmS 1I~H6o˛hUʼn6@( v!.q[J qbe8? )9%1}3L2E '8V;銀B"j`W%։āıd '( ;C~I -B\PBG.)GD"0$5IL+j1m|ϙޒЊH'#./nܐ/C4%21A_) 2ըl>=]a%nHI,VM8#~.Z󉹆y2@gHR]$Āpxyto'D׺ŔۅJ0MDKD Q#fqr`pt4t_W'D )XDOH ( 5Y&Q fG/GGh5#Apj ̂%I ?7) 1uKħɷxD_J6!ꕸ +JR4&[! Bz cF#5#-ޥ^)=b=*H[QMk=O>j#(;$t:Tݼ]OZ ,^}HQ-OX!`p{VbVZԠAodt C07f #f҆htXLNLj+62b|i$HlF "m +q>%>K2Ev&l EJ"wFIM}&>bXlؔA J:nuj0=+0o} =0Wa!d1v7&#Y-}Yȅ5tA_ S4+^kWt^fkR@3$)Ļ^3Y4P+uڳep݆It $,|gᆏmYTEYB^G`C!KDllg=Hk?[||㏿(-}!( A?ey} Fs@_@EŬ '` ā%SGQ!a|^nJ3 ?d%Єš/  '|5$teM$XIi$ˀigYa%Ρ$]zΜrb9͸%@bSScƭ/~KģI"B? !0LFq&`v݈JGKI`>O1t &BM""o7 %nVY!сQfa Ԭ?^I3 fGHٶ#H]љ'؃4M wm(f`ҋOm{%f%r(m!#D!j~nBgMI467, C=Sc%zXk2 .H@A &v"X5x%7O\w¾?l@*luv K>߰~NzРB\pzh8.Ԩ%DFЋ WBoeLڠw4cQ!*3dlbb"cS:S*`%HWpJI 7/ZR1$41d"R9B!q\ )\7Ȯ LtΨ-DH W)Oqln}j6|5`Ջ GVgy]Ӱ41<1]|amn&MsA+]8 BH6YN lJ;I-?́PRch D3Jp%b%5RLCc@I$YƑJM.JQȁ7 >B."F 'ӥ$$p:!6k{-6RV>"&>%5 HCm èF hr3테 nfK ĺQ!*ëFJBLMƮ F ?cY+nkO7RȂ>+\n6 hsƭmFѼde,f:q$:'~gI@}Ƃ3L%^yJx"%@/HSۧ?+R$6Jx/Bkx$%H&M#|KFs1{M1ioqe.ǣzŢL^9o\T8.Mט5"Ȧ$ZWb/+2{KÁώ(]bbW?G>bDI#. q[&Z԰I0]S ) Z&h.ICMkY%S<)x=NqC6nWъH7jR~Y lb1-0TI;:6ɨ*&+ѯ,ܥ62$h 9T1ɇ/-VX]XIMqnŚQ8[8%!dɬ4Gh3]9qY iڙ&|D#m{,ÿ( K,"Y&c95J&TJ_򃠌8NJ (S̽\.Aps^OZ볇g[wzx?~vN"$$&CՋXs]KW) NYk[&knJ H关DD+pڄ `bMw,.UrsVbڮT!L) wf%Y aH'Wm(Ke%ClE"F!b+H@JFp[$tbx)QMQ:wi=+ G_ծ> o,.'r(FLHJ9O!sOِ{EFepBp8\#Sߖb"/H%:J|E_7a%v$^ڒ;Y Ȉ氅i+[ގ(jU/ /ƷCNs  D`]Sppi\X-uk3]c{M(J#MX -EMAO=coK,i͉ {tL| Go#S#tuxSW\p:%!~^JsnD#4S@9=UJV|cJzș)%bD54` H^V D`KIhg73m+>*Gϴahe%ԁ7Ѳܶ璛0pʸpY(M#܏οkѕ(+S8_W:(g%mO5möAdSUPet=1g.UԳ%+1Ƃ@^HG9rDꈡR9!D`#HIEwoT(:؛'@+.)7nX Q[lVu5NDMTB[\ntY ԧ`N8y(TEBvdBhXf%.+]\tPBY»/u 05tcJz"a~6fzJBs*?/qC4*0G$q  iP-֋$-v2Bz8lHC9Vš254U,nQ=ճR]-@ƮyemZDN-adUU/j5Vb4(Qv Eȫt WPzn m%CfiJ{Viu$|+ն{?6^V_ϧ_ΟBI$"l54t%Ē)*Pyi` Ў>Bğ?s`)FwAI8sT` =$E8qr[xqVB c%,:L"TU:NڮC-hEI{ J9M@Fq(pE{u Ӭ/kG"nġ|(d"GI:+QJWi}tXxD42Riq%RP:ZޒJndMX%D ) g8B#~ Ljy {l=OPF܄J*ަu28-6b%8kY kZ{{wxue%"+QfVd(=FϹԠ_#/xC/3U[cm<1 =U. j#vݿb!asEPަUbfs;7AVbU" pKJP=BI/ 2CH^|grm e%Fmjm(>@0eV3Qr%x]7d%7$iO>@x%B^_-1Vm b%6mh =f# QWILjDStQ(JkŶSڼ@H>$WTXv8b\S$2l m:r5^c%I~+12ZrJX{6m6b5"V2_bOVPlNy, yRY׀$ vg B(e-Y:Z硯ChwȺVѕ>BKU(][tD ɇ@XDiQRJت]Cd%\S[j#G@LB@9coB4eSy9Q~A",u[,Q(F JiTi3VJβsSǜ=?xu-:i V0ҽbPϼLbkHGT;,rm|3ޅ`GW%9TƋk{ W-n2x ɐp™{6bD>&dzBW}|= C|"@N p)ir,*͍:G\5VpMAGPE"\ ,QO4.s{]Uϑ)/)l sFtR`*㹸LxW\=AotL};?}u͹.dķӟ,L804Z!vaZUU9Tո͉֩\rZKӫ[K j4+Q`)F@P/o|<8I4$5KX.0_UoC_bƷ"\v׿j*+mMbQid\"^2M)>+v,zh:szNsjEk˾k?ߠKDo6Vrؖ(+! b↬D4|kD"ӭ$^>nKKN+eŋ04 D 6nuVg}*\˟- !>E|ݹ9R%s!["|DhG$;8B0 8HI?a߳d]=ysߺh"1!C3R6K J͛%l9( )+!zUX! "3pz OeBW.=. Q1"eDe"1юH'GLizJO<2./ci/U(գVqHd%(C@Q]VE!(_~9#tB4W7hqrv]۝8,%R1D=K+d!.+{.=zT"ĖD`|k6$ˡ%1.qΖЅ)߻*'ܸҖZK&b8Z%*=S0A"@@%!^vXi깏!dIh^(tQyCRȈV[щ!F!%V~LѐP6Kp)  wK GR$"| &fkoDHyr@&$s"|d׆) PU6n[Q%=]C%@d@1qUdiw*[z=`.ܬiF&DErA"@n9%!ެ rR եY4#UQ JQpb;(1{L Q Y(éBe꼫.=3gI)>7v0; M[E5RLDkG/k]2v:RQw8=( oh&+%KHt'*ܪPs&´GnJ"g*Y Sܡ "0BS'ҲecWv74Hs:FKK\"CjՍr<{G|g#{ #3]mz%f"*{m7庝1|\ (RB 'tN\pLU>{hIꩫZ⼱,g%ULJ  >7x4F̵=*.,qz6SC!Q7ʷ_fݟ%萯ȷ! !Q:Th:` 5_Djƺi}AC*+Nv2_J&hDbe&F$sZZ/4H*2ՠzK=H\QQb$]`=m}D'Tu20h%݅%+qٷbqˬMpV"%DV4S~=@9勫l]Ru|9x%+1jPzBĥ|7#-%Q2zFn#%v'3-x'KnBNK>k4ARTF&<{$0]¢^Vb2R+滑erVsp!(H]eZ~`홞]{4aP%7AVb " pS'2S:㆜jO7D+B8%JĄOI9ԍH\(;U[#L.Ɇ4/\K2mtJINV$E_qPg{m苣AfCb$܄J!HIjeKN%|Xڢ$  ;PlT=K !VB0n_sPB 89 H5ѕO [_Lk'[bXHaHpG]4\ ZJ%$Df rЪ0RC!MsE.+'3e O?jԱ;:Kla%%Pj%d*?F%|ݺJ+U!+n"]ZHVV.xhxsb(m$R'tn|*M_P7r@ؑ7 hWJ+JBo1T@"2S`7 y G u (#JNp|>1D3Q|$^_98T̝f%-1X{X(*d*?膰%U",Ҕ,O$uO%D޵)|?3{TH}"@Jb[^rEQt{ ٮ{CEVB=7n QQ `Ǜt\Q"پb/&<,ނ "2SdUEV/-1#) c"? b2JB얬e_LD@TBX Y2ı2+VA؉@%"ˆL-2q ԁOs@rsv JׁP]^>hhywW(FC p釳>(z ([%/F!!2B4#;|+)4OH` 5rN3$Gs{7ްouTޡ!hn\o6e%h\"MId(TIO4΅m]'qmO45j#[!&-17x#HQR,7 ^q Ɋۋk+2=_vJlqW<ߒlyM0&(b($&@_P~ޟ K{+dՒR]H7f%l%žkZ+ᾁV@1 jЍ~?Jn*?huh[6K,arD+"0Bࣟ!kFA $9 @#P9f?>AdlQ2PI!Z+ˆM?!еގH}OٚanGTKc`OB.S]ĴqZHt2BdU3<ߏ#D[LbIHFaqb+˅"B1Sf"9K>" ""̔ĝ;ן,] +D YgpGhu!Rf'\V%&`/KpH ] (/g8aJBk 6%=1W; d%D1-5d5@+4q7XiAq$hZ|38gdJB%O`U!ND<=ϼq]{}7Fw+xv%,PqCY>pG8u2p musQ+Q: !*.yDfފ@3a)9 [$Q2:Ʋ:]پQ:9OdY,TBi*?x3 CiNKe-FC2Vp ֌nVhxrJdV"87v;ncvhxXDO܊ J}&*֔8D֣)ztH@a% + |)w}{UQ?%%WH{?t k&#ށ uOHm9nzv@ 2DB"\ FCi  5#O"Fy"@o#B_ӢRmRFF|y2JB!4.P!iEdr Zh5eUr>By1ܬඥɏJL1b%I~Ü.-B|8=_0mЦSh9bω+ xz!nLG8SDXh45 DAϒ\=}"m@*4u:)z8Qs!X'^O!+a eztKbѥtYBĄ>jG1jm/>C%PtچM;H9C4;3b[ so"C"XN" _ΪA \Dg޸[6 ޤ>_2'm#ޚk DSHfK@X e&^{zP{e!K~F+qEWHlM7T˨9LPY q@%A J]o#A X+D3"S#2ppmꑵL@XړB8BD4.!D~2{#CJ" i=`{4}8%[>jOf~[(K /Ȅ=jㆍKl|DGt{( )W7!# +%$#2QN\s254ȽYF|P6sN,j"7BGZN1L`j]%EmӈBQ"mN^mUhC%ꕍ %t$\ՋHVg\]wp:u7Ǯ GxGh?btD)i'oZs%ԓWG.p߂Ή]}cjh*aoLRuG{*&>*"@0>{'l.k;/3+PԃJD8ЗWQnj{ݽFڶS+H cv=m{M5> +aw5(~%)lSFrOG[G#popU-|,{f r<=_~h6"D@( g>"yB AF! \2 Ք^QD x,#b^%Є)~\.LBy( y-jNC#ͼmsb80!7YN?jȣ}DcHDF ] ]%3Є,܏Gt;u", gMD*rtQ[':}T>Y]r!t,N5C'&^MV\Z"@ P2B_Bw`M]Y㊆&4g(buq{7ppdqkUv.ĪDʃ0.z[Mk^NY 5xDVBw^lཷw]ȅ41qVX|C![B~%EK*p#1H)CxJݬ~޽tT8j>ß(5K$ZGLcwx X~u׍+X5eq8KDZ Gg%S0AӏJ(|nغ]h>Bų,Q[GD tlѿ"?YELpބR xxJ"zs\Gf{ч{^ +QĨo+WbbPb+1F8 OT-ݫhهX 1:4(D;1mr Be%1q+m| a%4+h}o!!VBbBLpbUp9 xlJGD?BVjq'*3u)$LhYŗCHrNr;+ @%.,Ԓvmlāx`%&n@tdevMJ1q+1 aj:N۝#+nFmo+{j&D`76+ov4tzJh֫=jir@7b>ӵ#Qz~Ok>!Lm׍CAH,.iRS m!p[K(∏@Z^~ $;"lBŸ~Wm0s@5yڽ+iݨy*aB:ϛ<KDusn߸Ifiǣ$kY Mjs,.dykA5b-P]MV+0]d#⥪EރÉοu[mٯ4f6b%p8JI\\ #-按PmL;vAY*1c(oʅDB( 0ϻ95۸?<L!;jIH \%GkGY #l U(%1Bp:+ں`~=J8|D6lQ%UZV'&wѹUM?uES.i }tA2M!bwx Dx$J"zs ?mӖ`uÔT^ ڵFAWrD kܾ0P|_JL}p"&lˬM +e>ɠ/T JoyodJLq[ʱ=X$9VB‰ n߸h$|ϻS0(=G7qlvW=B_8 Z>+q%+1P Xԃ;{Y YMz'Z|X Xd%6RuHm_ŤCœ DTP;Sq@ n{qQ +< %h-޽X]~Wn<ҔCQV5.̧>tg%p_ThGzmhBiS>"zpZ+f%A4G&pgN‡zJp,MJ)1Wt<+sCH+DB.MIi}UhJ2HL8w>h vjquiEd-nDt=D4LtU*֯[墫;j47#-#|'D%UA; 4*R## i>ݴWJgb·jgm|Ȍ3_b|p\8!D$xJC/ W>k#u((YtƻpM?}VB#ĄCI.=9?u*<$a/lH,498C_/T*R?AIa8D3+!#b"J:-_* Lݞ NNҚ}(nNI|,N+% ֓.i ʿGZ/2( MP];\E"7nIϻq4O)+?%>.7. |}erqȽ'P?MMN'&^iH\ *)L>e߄k()ATC]S&#趪88BkvTa(|خvJ0>;o|#Ą1+ݮ)<ʭ7`K%D HS/ Jw}+{akduf=tTQ^ %Si.L{Ź-T9OU!=#ᐒQF@%iC$_cY#6!jn2"1`NFTA y8nKvbqzv yńs# 0r\*+ͣddGB^eQwV,o5L1"@!)` =N}w PʖV!bN$T$ĸMLjWTY [0nP~%*Jyn"|ĭF$4vgPbrg2V&i,ȬC/yָ|`1hG]F}U/w..! eV%v ;}V/Cf M\`+;W"Jx)OXloy8{3k'Q'<5l<2%a~μ$p?Ϲ=rU g%Q%#n@%+bh*%QϕwZQ"p ;+1\{4xY-1>+1>x?Mw+XmWJ|n|>rcJ̻r-*J؅ۿΞG)QCdU.]=W> D aDz~Mĺ_ܙ=vڢ:mu Wj[>BthBu&$=$b#PS{f4^Yug%rbB:="CT;#p@BCӟY,g(LO< Na"@FOiu Ax=ABUb+gnq殔8.K L5j|jk*p1[QjM@.λP=ͥ鈄qobN*X7i2B0{+!.UDJ\GphhGhƹ{lЍze%r2ӟ';") }2D<GI (irB9S4mp ~o3M{IApeS%*{sgE^4ж>(MӍуĖw|\;"EXޝHLmor;d%JbB#UȔَ9u5T0|DAA( Q=GKtV>Tq[D>b TB@{.(lՏ0"#rmYqu~rrG3^ɗ~7BgITn++DPtw%.b^TE$Ǒ@[#UD΃|+!GN8M!`I;TB[&CgF렄+x '2P.K\ iYUFG 9*>A IHaܰC<[GDYط7摓ԢP׉cSrɑB]) 8]{ߪRJB]q0(Ve,: !,UMhKG 70az'PJbzPH;h,>62eve1S 2nBI$`oJBk hGʧU,t+?*}Qn|D.}\E^ڪz4(!O]n/fXp2%%xKfC.?2eEb)՜_sIN *__Ǘ'*}b\C>Bc)YttӅ T ]o~ K?]\N8UŹ ?q끬k(rH .41EeV A>6:4-+o\K.,]gn=-?hÀp]{{s)JZ G$T8DWEo41=Օėp(|,] qp%UU}ra\H<me$<%`}kL?^ aD@Y;*!I#)"@N @JjySDq"]myEL,A.!#]b%@Yx>+qq K:At+mZ?+q>KVe 1$%dXyZ;acX1!|pSӠoK/3ƒP7*+Vknu_G$䡬3dJI>"& "p~k7s|=v>֛h|?8Z|S؝]bSZ~:V (%/|Z<-/3?GN bhP~pG\c=JHI3ZUYiۿQr >$8JvSpnÂrtW?]9s*%Ֆ>hE>MG|Dhqw;Ȉ]SeC܉RLN9JBY1QR!ysV( c)nȫivٺ +!F??-a/`D%~3V_%qbd%-QWCu(%ݜ&Г%S$V RT&FF"@AĄ_II7C4Wz<(G$ʈ%7 6MAt!*,zS:ſrq[׳깒 ZXnh=РŕiUrzD2]lY2dcJ 4 )X.wY>%ڈ XgJm>' #ұ/^ Y9XJB("a%_} e|Z:K^qʈDipb[YsvXm##}mk]1o}O DY %Qg#(D-1JYƔ$Q:W9q.\r]ZCz #!g(YB*1CoXOGByve ?s8 L$RZrFdV@F'.+wſDBGu-e# <%~6e%Ʃ+sܽow,-QlQ(׮cl]JĄ#-lEH%8]-}ЎZT;.#rQ'JT:s88(K W"x;M%*y#ڨP?g(Y=O\ԹUi' oJ,Uy(OY GL2"3b0i,!Pr 9b%FWHF Jp*"@R/i#66K*Ԗ?Ӂ70$*'XX<7pO-&o!/fĄ'JTLlqQ1e?GUZG*C6t%]Lm %PjGcOGBG$JW+VbzD UZH8sHq#'$}*<5= e%ι=7GLpX_ k&|hVV"GL{7t閍nԣ}ߓ/&&s)C\a(>JO-t1ݓXkg^;%G\w'&h^a]9F9+J "|Y|bPbT+3 A!'ALp4 G$P>B+1i2 J6pGηY nv2˛T^JGX hKWttM2EDCjJܯQAzHT'\2BG{L_,!+k8{Qa`h)1!|A1"@7|sjdP랭5bB[FV`r ]Uۂn 0o g 8Rz :BLC9nw]!9 _ȼ9ĴSͶSOՎVM[}/EZYyL-jwʀ>tሿZS?gbqi"G~J^AR+|/I"쟯Hc8xBĻ/|߷M좇՞A!~:2rǁJ9I(+1%&JU4@;0BLzoVO~_ڒiUn$E2B 4>bD)F58g*b7PL_\H"J7nlY MGB6h8|V%ruCn`D9;8F%ؾQ3bS>bS%* Ay{C soqBRQ]P:BYm n" (y[J$}<.trDԇb%$.1AV* gVbk G ʗBLW&JVB-n&N\b+Sb1dO:\ |I3QDa;`'JRIa"@׵qc}F(~hk.:ޭ\Xn86PPqY]pNǽ-o(n*ײ<-G| TIG"Xب)uڢ:lONJvz  }9FmB1w@ N{{[oCȈ>Bmޚ"))n軵R Y?Mqó0Т%%!J9ttWt2=%`[ Ek`tiOePԭH[+]7ʨˉ JНh;d# Dee.3QstM&Y SP?OS7hw9ö*lDBXи8+Qg"rÍ7^ %qЌ2/*ˆY?w[Ѭh) яa/j݈N 3.${f%T>O=JIT #N:b/*֏+!*1~^rײ-e Her&*loوۗе!n ZY"`:,EV~ǃx 2J"pJS zogiZgkNJ$KhŹLʴ!|v^00+qӶYr@Ż+eVK2%_O[1NYv#{KJT]bIF"Z7uN鴚+9ߝ&}g:ς( x|pIIиJgFbB  @JmPNASVRu_,p.-1e%|JBO^xM6AS+baN\e"0(b#𠔒P#bbCYɟquo~O-Dvr_@DkbJIȒn*՞:Vw9S9ģSFH _ M3R-K DGX) EnBm8"(@A_o۬?7+sL鈄wXkߓߵ^>BIMW LWMgթe,ښ.%aD|DIC(!Sݳ[lL+'(V!~!AaLWrh(5 1_L!J[6mh*Mz%|V#JdV&GlL{CuQ/O`P&d"WXaH{ܟ f{=DI+PLY:}Quan@\G#9u@Ub].a [bbTN*G49I>)s'+7ƥ?_Xj%S޼H(l$Bc ]7yW"qT}VBTM%J# X4{FD1esin=W^o_|"ٲ>JDY]ۤ#-RALtiJn˷B%BLT ]V0>b#[ &dtVT}vSJrd i"82E^\wKWmOؑq.+#Jn|X#DAxAi'0eX FgiOPfJD)*9rNWb i84+1u"% TN') {%aLĔ XӔڭiy %QzE?csIdJxHyXh=A&5@V,f9|CNX>Do|"pFJp rNK Zwݮa%T$ 7c%>B?ms8V%5uKI _w nSW7&}q3/ՙjGi)`1"- ܲHmgef` #OQr]K萱Dg ,@$:{Pjn:.x\eo54A>"Z~'DxXn%#Z%F_Q+azR۰\D>%G?J$(nh$ B]Vxoԇe$-\UCCKb}Gh|D,BIHO3#=-"n_M9gB̍F*F wb QZbS-a.ui7A*jE@g*EVYQ*22V"s%$  DxՔ'Xk&LrACu0&B *XJ meUen$ʼfP:p/p#~_ZCwu>?2 (b{[>o)rPQ'筭u+?_Kv$sDpyQit=QJBHDC_%eąD>^;%qAm#jPm5-!j=|b>d"4R# d%J\#)~AeI/Kt;+!QTD$* FEJ#[%&j$ p |nEqbL[L?Ɉ ÊC +AJ,x"@ &G@(UN6R[Q>By*}K%(z4wQEϟu=Ku*L>#W##p!>Bz,67HB(g֭ =/LMaf_iKa%Z)I ) ZĹl&G(adbGʵhUD呸r27dŁtFVV8ph*+>B huO@( N[ɈItFJLET?RI_) ي!ETXX %& )O $gTmZ dU+Ca_8ߞ΀GTLDwIwDBYnM%R[TP/Sx^T6H,WB4+#i>n%1ːsܠq @JlnTWDh hӍ$2t\ō *C  D %q*;JLےP?&'Dڍ[6~X+aP"DSۖR;( %`Ϻ$Ga\֑vQ#4/}(BeSYDX@̊(gR?~<@Vr9 DDzh?Z ]5]/W!l1 sGĶ͔4X+Ekr&j qjQKGXBahpʈD(e"iIW ӬDez̥-Nh#ޗ31\}%x)J"@RGʠ;"a- J.8*|DZ? +q8sJy<3uǦ|ĖA 9(1;8"Q1zݡStFGtD)HD4\A Nq +J"@F_>r bŔ-,σ7hT|* ӃQ{K"-}v i W殹N}>9(+ڻZ@r&BБhG>DB|Q8#l5bSVq{#Ls`֛•N'V9KʸFNBBfN JpP"Z!$(&MBDUF*Ĩ ćhhFD-ю=!n#u"؜YdDb8N (B8p< Ӫϡ[R8fdDBMHXD|MqOF|Dw6$i>EIO1b=s+ehF$fj?(FKmӛq3MDE Hߛ-tSJOLSS #s6, L[DBd(w(}K1pFsSPU;q|yhC(6W }5?IsEB`~ܖ#V+!:\ռܭ7 }ҍ!|C  D( %ۿ+~ 2$ rBtIC|@+ztI:Ti#KsVE7](KST C:S QQ>B~M?'>GKT%Lȟ WKSCR[  ?n`nDV5S\ҭTs)`LR0 2!tRn~AU-"0{V"AITx%_&Nfy*$&d%|Ahyܹ|5.QF=D"7RJrtBs2StUiQAX) 3@VB w89 A_k_yo["@%'á#$&GëƵj?duֽBRD,!)/\&2\r!9\zhH O؞*/&7>b˹?G #*;8:LްCL,T9.I(MtʊFW>UI>Uɍ*L8^ S }8&dhx[U\Kx=pJbODNq]bL~TSUSP5&NCT&*2BC$C%14˸@@NpL2yHX/ݘJPb+ ZJ[>Lf%pDJTOE o:x_ %[UA 0V.Ӆd"}"pؐшDh;~YhH0)'jWŪ`Sq(SӦD ;.R` N&P>iPp@DOI, Op'n2mG(E) #KRˑWDXwܐ"kCdAZ-+QNF܄ cKI"@@)zpl五@T͞ܪv.#)]2BM/RoioVz&(H,'S:&xNBxpTU:#VbP+ȕ+\埠RugsGU8FPDTzJ bFR⛏4X'~m@{o2.1r׬m8AIkH& >%V$JJBJt|`}Y#J" y2z"wkfoM; $991"^ a|8'; |Q26zEc^v"gp~ѧs!YhB1jY>8"sG.!wr}spsP_$dD 5>BLLg%D`qcW 9HB"@؈$`N)dDSwV-2 ~T6P64NOXR}ߚVhŏ%&F^J!q@.P,yn\bQ^#>BMعӷʥ0NĿ'h..[AExybJ"1"a*+a  U;$BVTgII+DzP"7%!%vVbdh#)'~`hD<($r唌0ZVbKSb`.m#Y&0GinB yOIȪ@62u,%+GVlԟ(S]$Z>:M2C,p:sry4,kC5Ȃ%!z^JB8Q7tv(]Jb#!FII .$DB+wsd%*`ڵ}5څɧKڑ?@@wA@i=Uʏ2ƎJ_S|z YWaAP$^Qj;]?W(~&U>%1l7RxmAsU+QR|XQ=-%HI<ӛc!D9 %.+1jy`_=&3%eߋ:'dL"-wZf^׮^|*SZ~@CàX 3 .>!*|D^ϒ(%eCQ$BݻyTDy8se( 0ܮ Aʇv>B̑x=oGAĆLӘ)QpDkQh(E۞S_6uiJšy*ILSɩ%1ŧ-ha$2.CD&ߟXK|hG!>H, Q|$#() #P=  $lbJIh,S,3낔+y`D<Ɔdɰ1Tw02O +I䇼HUx8 P =et,!0]bS1\ge0OEo#:CTZ/S֖.#Nso"I|&tj0`':PR 4}o(911L"DfJ=~FDh>,Vx\{%WrՀJNzCm#ݛ&ceQX %&OߧDi|D^?<}"HtDB\J,zDKjlϸlID"FDzawV!BZPT@ю(*s.}PVb# JsQ[Qy(#w|8>A>b=J7qnD6(!Y.c%> 8Cbe|ĝ_#^MtoAr g~!7"1ھJ >LJ}6YXgy-:J"@x &'w%ݽ'ő- a#Doy%s, MT!o6 |TUk!L?|ta*'D[a%|an9hM+IONF@I"@= ( ;GG$V M] P9c%QG/ȣo":(D>jD YLڨ "#;b nҙK DqxuD]#JNHgB~<^ӤL.mWx4#@FM>H7QX$ج1,b DgER/76NGfU2(1ңpLi*0.GL/%K# ­nnƣ~"@ x!#&$ljrΜ>4Ec[B).mQrxDnFލPR< %+OVBC^/%1+Kͧ/.ahhB.g[* mx9輺 :ߊmP!>B4sF^ ?zcHqzQ'"@ z)-ݧ%HsVHIB,٨HŮ))R{c6"T~( !\pX12k: D8CLItG$r };sh EJ>n)"O]ju_Ѽ ڪ VEHC֟Yp< 0H_S^'=_i>ȟ1S6D`CQVo힋WJ܈ r:g6FZJ{7%jjb?o9EH9rD@*j%D{CĽeDO@tGھ-sy.?=-Jp\> EF- >BI"@!D hRrj&DGGVw;=ݵ1e _[UFw. 6\L!ҮDfq_>lVBX gW8 D5(ung>{7@y"#ph3@)yrp D?tJ!#Ҵ!OKzVe.psG+Zˤy=a(J&E.L0-| p':s  D^)%##dPB gh\i4,ÉF悴».(DPŢ|i.Y }Q8uoG$p"@ D" @JA2G$ ƶoJ%<HfuB fkKV ֆɽf|ʇHpGw5^~4X*'D"@R|3+pDGJTse06 '!|IDQ"XAdrеr`Yʷ Х#ܻ"O1"@ DR8VInh7kL(%қ5V3ݞނX9c%WJ=_tc;D"@ ){[mh5ڠ6iCɮ*F|VoQ"t5r! D"@@) e%bÕX fJh;]%YמoC>}c +! ̞##,F02"OR>+^C_|Da.$D"@"@J C{.`|F|#h-V,Cܨv*bVB)?0 D"@@x]<\JıYtq螋 ZF_׺u_o6q>+h!&G\n"D"@kCuQj Gq569[ljKC-:A5m傝]dԢJ?#6aA@  D"@@Ri^Xm␝lpbn %.AU#[r)!TDuxDh#e58E"@ D4k"Wwm)=Cz}۵LѕwTsNwTG׷%,m5+Wm~Q(F%H-0/P^ZiѶxj%;Hr)C D"R[`|%t) m'oJNi{m)ҥr{[9ҽsyH @P  D"@:ܸ!5 p8Iѓ6*7,zv=* Ո27Q>Cյ+!D"@ >{mU5S"@ D7VjƱ'i7-ZNW(-0bZ>> ܠf"@ D"p $uY]+\inqQ;@Ȃ̖@Ty9#EdV\[DYgXAk D"@R>W6Vkw'L)9JLʵ^j˕\?Ƴtr@4!&݅ D"@+D+Lm85p+bbo6itT T4+*E0b#)@ D"@RG^zVgRjz;9b0񟥰2;'r,J L `K-S4p}׵N+r.ncWjJqL(I D"pJ!tN&ǾsmԿ9.{' \`Y %5/\u>\"@ DFi\ hkwb~7ry38 '힬+Lh41xt7kT'\n){*!D"@Rc q>d"^Qq6=4-)+1%&NR"@ D J}5@#l&b"7th.0#@|Fd,Y poEciVbDLV  D"@WAI<}s:I8 |D5]бz@ y\;+r,+il|"\N D"foSdQF$g;: SGTwp;ܚ1.֗ߓWb+)_>LT уȀJ2}N-S- mBn8c< "@ D'$3/z5"&^G( yeE:-]d5GٟO!,U+d%_q̗;+Qw S D"@JKDو|LD^J??c( qpt(R9S LJB_$&V( # X!#ZA̵V D"@WAIܬ1 BK>"7Q-%z.)Q=XRjFS%Hnj.$6VHSICT:(:<$MI|(w( 1NFCGH"@ DC-) l\EXg".JbG1V#3w1g[Q@tܸDAk{3cdH>,"@ D3$9;M {6ht( v]=% =JqK]a;8 ג0~S=o}#GK">^E)j8RhDIFla*C<6,0"@ D<$(Y7p"&vyОp>(DM{]LbA4+J( в d D"@RWM[/U(".Gt)Raw`z?ѩ(2,ʘ²0Sx) 8%B|#D"@ S>z*A"pWDu^ ȟʧo)OU⪃R$VBW=S"E5vzr#GP"@ DfHI vD wqDOW#jø]>hVr:aa]p= \+J"@ D4ܸ 3t) QowEr7жnЈ#pG`hN=OE, 5Q(Q6ݸG| n$K D"@^C3oz^$[6N)>_U&j#`4:Jÿ~?ujF, @}Щ$)(b'D"@xxyYiJ"G(#$-)ǖheBmݰXZ[) 3Lϑc]Jb}8B#%XQ\N D"J %J}/a( !$J2B6JbjŇiGr%&~>s=_S']_{hKS>+᤻$6bĴx(@ D"@R,KQa{4&ljͩ$GV%T^>D(O D"j7^moG8La)L>ҷG$ԁAe_5ӧ7xƍꆠ Cl#QP"@ DZHI?F>Q1/ +!;ϱ0 #@DG D"@8)RgitɈ舄Ɵk3}x+o/s#e:q +ƵD"@ D`RRrKۦ~\mNrW1@D?htfPBF8+a#dbD"@ Dq񖏛\PDyӍjDy5ӿg(Bď}7lk qC7 AKo$ |(> D"@!@Jq cK>bZJg%#j( VkY6ᖒ)+aLĿ~^~J+\!,?D Q"@ Dܸ2 ҶCZ8}:To|&d%Pkp"@ D'F78}ݩyN =Gy|5F g"FD0ETdoEqF*"@ D"P!@J%zH-9MS%SQBF(1u,FNS+ D"@ D %X!0Q6LItm;=Ҵnpxx-ň D"@CHIaRutd(PhHx[C`dDIUmA>Vz# OJ"@ D"p$.X34"%Q'L?92S! D"@ Pra/ntUUP?U@wDb#kP٩ nz8 X)Z%D"@ u;_گVkKWJCI >3,KU"2=:ReBU!{H+Or"@ D"p%!Nh_II\h\6d$F|DP[(pDU+Qq`?{ymwuIiqߦRD"@ D`Ra9"T ^g@НQ|JBt9 (T e_JoD@P  D"@HIrA>"떖jY$l㽽+QnXpX@I D"@.C @I1ʝh&}LX D"@;noo1qEz)}=ıDTSqtwOrO "@ D'CN GچsOv=lg:%!PF90bpLIu%z[ +% D"@+A`BI#eG+Bˆ8ooQS 'hՓh $aRV;nv]e$0 D"@V)>' DhU` i00BFB D"@#| 7?"@ D"@!phIv-3/ݸwI D"@ D;dijU7!D"@ D;FF#B׈ D"@ D  l D"@ D"@*HI$ D"@ D @J$ D"@ D"@J5@ D"@ DR7& D"@ DR"@ D"@ D4I D"@ D` "@ D"@ 7@ @I"@ D"@ Dk"@ D"@$n:M"@ D"@ $XD"@ D"@  %qi"@ D"@=ChIENDB`PK !tppt/media/image9.pngPNG  IHDR&>sRGB pHYs  ~$IDATx^mE7oG%dimhfeØY$؊1{<Ãk8Ip&8,/hwyQ3ȣUqR!7\ꮪ߲wUW꾻窪j ~ P5iӍr{|z}/|m~sd 37{.˯EQ_zSiר_ß:voR#珶\\;\qݶ.?ںAӫ\ gf9Z%ź4['oL!hV tI˯*_bqk̯L@֯rG?^W}گ"׷QG6/կv>:qV^.#?^>}O2puD}*~;~9ĀO^7uס)v9ZīֿtD){æ3<؅$ǻ GrϬf*#|k:wתfcvڼN}$o9OzwO_~cW߱)}K˯O>⹫ /' puP -%@&&`W=:#Dzo }f*EBȟ @ IY3j6 @$ @ @IbT-e @ڥ,c mS+sh >@I I$ŋqOHM @@$^@Xj[Pkb[ "0>?bT FI/ @ G1 $\]8}*sW *25->DgV|&77?j}@^=ebnݷ&/UÞ߸RQk](_E`}. v>J-Q765`(b$Q'0˃M=>f 7|n]]Ĺ[]J>µR_xyEQ%8U*!KΕ#G)S3|UF7k{ $)kɀh&JXG_M *ɧzY ֛he7 _jT[gjA2 ڼN~KY-'^E^~cQ7̹OgZ]״l֫ʙ^ƏM VX]-,)>#)OuVpDzYmѵ%%0{Y~vdJq5{œF6B2CMQ"ԯ2U F$Z;9{E\Uš5#zM'zj3x=בv>jGs̏ףռN= 4sHo5Ay6! /#m,ba_YU1|Zǃ;9;'|dL 蚻6+J̞A" AmYT/G_ 5w Մ)GԯGM0'n ʚ%L(OyKG\X%=͏,pX%Et8}3yq_iG}$oN>JȯrPL"@l^J7R@Uw}_MDγ#+JA%LKޘÃD뎑Vh}R4]q )YE3F]lO2b-nU5T"4(-EhB!YF41߫#Zw7"zD1VGrrOY3Jhi}v],agnj?uZ+N*mӿ V Go3oq'mAb_7 7v7, b[iEhǚs:[hCZGT~XMʓAXU 1LF,q $jpOZ=mKk>^)){%fZӫHY @4w{boDux+"5GH6GVo?93֪!7"XAsoN}{ t#xFoEh|k܍a}M1# e&X#]֬YRL"n\WBkI=zZ !64%DC\S>%CSQ,?ʸg 9ԺD)OKZN鏪kd19]ףٮ͉=-O6}hәbZCsm20~X@)',iaajl`o5{BؕvQD7A֛v?otbYj %aw,u)Z %U~ I>Fql^ ꬦ'OcEJ:jSv|#rR6}15+s L k__ ˲/qT;zJ5AY*^J,]5i=<;I,J4}v{~o=azQB̊W?qր #Gb-ovT;n4VoMUBT֏͗ AJw00O4? +!$kךZtgmKH0koa =VhU"!S6Z7ZSbc Sbo/*eT$f5q7?UfHqY]xL吝XYHoe ~W"_kcv련HזJ>u\ToJ0_M$pzY嵎+(t!'e٬˪rsNkOPQ"'eǼXCXT-MGsW_W{{~k[ZIqc8VOx~MW^ _\]Wn3vM>׻y'ic?4oV vyy^;V:b l4qNJaGD}0qQ/mZ9Bn.{i.]i:,կMׅ5eh2Xy WqYLUUoGM͕#ZE%|veLt ʏT%ֶYR{nNkI4{MR_>πۑ>O]^Eﯭ$q@ @ $ ZgL uLu K] QpbCQ>)$~ۗ%m|?/k҆ge|Yg 0_5CMyf3z LL=24'FbCQ>)$~6?/k҆c|dG؎Rd8uĐ$F @F@V`_h4 1$K̸Eh!ps)$i @j% S6H5 Du @I4wMR͂? C`$f @ @ȍDn"pj>0!'C @@3@ @ \Xm?c"!Jh*6]:e\Tm׹jwϴ+S}M:f:O)iG_ܢ}ֺvztI2|>]{̍vгR_?Txh=/OӸ4nD%^ zۀ{?COAb(1 $XdK)yq<۹ڈ:(nuhM^bDWjHdM!zRo#zWo/(/(bd<^b=H4|U>f,Wb7A*u7˫.Wzgnt=h˺ w_#gnSD_RGGH]xL=n8=d: 3oy OP  15˃ل_M7ַ%LD 2"}ѹ'Vk̶"CK0eYH2b3cR^T:v ;ywU"A"5 %& {BiTi=hEOQ 6H+ēm/U@ޢt_(ssP&2T%<<1F𬥸bQ\pJq$&$JM%)7uzBrǁ)5# mWO[<{Ooju(`o{2*n7Y m+cϕ6T(=^)*'œѺbH|x+5I,fXzdoHHk R/ۮ(vꕷɫ,GLnW.W,_]vuxs.#& Gj,c!Z;@-}.\ $,bGj.NU^VI}Di*ޘGqS6?R9WrŢ 5*'hzNtff9]B6]j:2(’ I#f=G=ǿ^,W}|תyD7ڼN*o{}O^~cZ˜u:ߨ_Zj 4AcAbNIB DՏ}o u[LW[ktܴVߔ5dOHj?Yvuug}iѯ S=BO1Cg,_08ѯ"pzꀚaNꙎzꇲfWAI姵~*3ST-ZJU%)3Q%%-:zY&|ϻ|p۠ի}9vq :ĺA,/{\nxt]suwvp/ÁKY„؏>R/%F}P[?Q/9h GWoeuʠ&haNZW,\"#e #U]G'jrVoD"[ʹ sK=GRy4gUTԇ?S/D_5d%X|T }Q6?zZnyV^M%Ro J?6OAb#M}ĚrI% ]pMtJٮyG]UjWeOL#ͺ;'Q揩Gȧ ZmR%5ZKEX)uG#:'MD0U G4D Qf8LHV=2j?u蜎7Ӻ0y&4<Fꬻ-G,7=&D!nx-$'K{&JP?T%̏Qzy|LZQccm|"{2fa)o ZZ,Sp|JrβʸZaJhXʅ;49k >֬_GJBGt}w>=Bt Ε7H!„JXbDW}- z4 % =o"~\,l1jc?x~-wE;nml.´f͉xc ~#^b}^ڱv`n~J$-ZɃDo?zo\n~tȂNԖchui_ExVj{X a:f&n4LѨM=šbWakIDԼ_L*6FL5$cp}zg|IגZgu,1d :Z2>&ZOVz-e3RWgzXvlUZ0kr1gyvb/fM˭eד}sגh.oi֎>#:cAޟJNT&|ֶb4#,nj UKkLkIO ®DeV?2hKX3oG]e^8FUIqLVU _ Ũ{5b7-\ JHfzYg]60G*jOPגh.oT a$4#<>e^┦*ᳶTgZ_-U@#hm^L4ږR)ږ7\(XKR-կj =w,$!u\ \|c[fu4;F]M'5,O˓&Uxק]}Zmꃭ:ծސc][D2nԱMe"XlQ*omi~j-lu*hѕ_lf{EdvPUwגpw6['_ֵyZ]}Aqټ:n,]m SU>wtG[D|WpG$b&{cWଛmӎWØ]z^A]a5+zb%uе:i=t׷^zAחz5%)F4,Gި6גP[Z\4! e60̏5mKx3)=ȑVYLGtsǍ{D/&]ږZh#"F(kIXJyFbةkz 4?]3ݣ,$ ϯA 7̭kF~ @{FƊ*r%@$f Eo9}z. IgE hM1 (Ib%b/"2!I 'ӕ-++ 6E؄r M`h 6A") s\‡b1i82!I 'Ms%HA @$0 C sH @ @u@3j$~# r: @@sgБMM7V=wѴX$b @ @ I EQ@ E`kXU;#@D:XΟD1C@ / / *A!P+$Z#K ̾s80q޶[٢ӵý%3  "7?~Z_?:V|h.$i8j s~\Y9qZZm5yQȴ[YsEz!TOuLc8bƒjAHwcN@}+k Kpl~ :KWvcI '̉sܯHTԨMZƒD.1@Pf&hlְq֬? sTkb6&!ixW#q1^q$dEgj% }o:E 7y |rA"e$& EL_fffW|HѬT $&X欇֙y@*$%Y@؏>zks< Cyeb:|P%of1u5>Io4?m6a|_ji+.F$4 @ ugcAB?8lxpވ$C2h'k<8vhFog[(v0@#mD}i$fJ@Tc}qKl؃ @j&$Qsti[""FG$bY@@^x_]ACh $R~mA~Dmq=  ФS/ T(5r=@`_6 #=v|WXv @X$eś֎!Pqj>`2&Ԝ @@.znu }D\€HFI"Z WGՅA 0Gc|$cO`/rb @@1$ E@rک 0l^v.gA $D:XzD Q Zufr #>(S@H`!H m򽷝oב?WJBYPzvQޣJd0@NnZ_{os @`r &B# nBHjߜafiꆵ[1t΅P$M ,@MH 4׏z'JAʊyJO&Stͭ`Ex<8 Pj$$w8 @h.LenΏzDW֯-̹ Eބ@NloFLv8y]SM#;>u㉌c6H I٣  0{ IFHh0t{oY]:ں)ƀ!@@raR I9 @pXvf)>4u@3*C0(@@dNYWqDIb. C I߾݃;9룛 2!cS/=3_8z#? @ L@IbT@ @MI>@ |r) ywOzK?J@(HE g!@ l>ٞ@y$ʋC @P7IGm'$)!@ PzS R\6?>y]qn0 I@JB%- ,+qW+&,)q=c\PiQ4X @ F @ @ H W!@ jkO> Q$ z @ @3@:UB`[4{nžJ!@X ?V?\y2|HI2 @ P;'j0BI~@ vv}@HDI"X.ß:5f@%pacY-1`ԋ @ '$)!@ YTqwOz,l) IM@ @z IE 0K|k@Q@ @ $A׀ @ @`H3@J@ @$ @r$q[yǃODLCIbŧW[  @ @Xli8>e1V+ocs!@D%@(1j 2=s+^B @@9D@ ?ST @ @-$ @*'cSvYylDqX`83΀ @`~;\;j~?`>'^Ŭ=ZGs!0X?COe!9M`}ۙ͋CȘD =cY\VNc.C @`1;-6  @ @KHt\!v ³OG+@ @K&$v@ Eᵷֳ=B7x 'GX*>@ @}=,]Y^ ;~uD o:4MkmNg! ZS~7HIbA @ ?ɇ>ZWoz !P $R" @()e{,bBMC!0ß)NY@ P*O{b']6?>y +\ڽD!> @!pac}e5!ȈDFT@.? B @@H6486Pb @Ȕ͋3u @AIN@ @ $fN92 q @JL֨24 I>@ @ $fN @  I ß:f_Y;?:~בۉ9@ @ I @ @I @% ) ILA: @ @ I% @ @ Щ27Gxd^ȧ{Ƴ0 @ 8RP$gN(@sӐE@ @(HE g!@ i {b76#2 n"$$v @ o_& $< @ @m$ @e~G̼~h-3pBI~T]2]@ Ho=aąEƞFWJI,F,e !dae$8 @ AL&$15q @ I|t̀S ׾'),c I,*4v=viXhi6 @`XrAɅ@(4p=P:Fѳ?a ,1q n:vl} ȍDn@ A6QLfP1b~$8Q  @`ac?haXuUOL @@ y6l@`H3@J@ @$ @ @IbT hvꕷ @`V@648@ 3wG0 @MI"6QA @ AIEj'pd7A L`מ|蓙zdFI"qjyj뾵4*xˏez @(DY[@ @W @$J C @s8}=ǿ9 b I:@;0#MS @ dMI"\v*4t @ LGt O=VjJgyO) @$p{+BIb!!d<"L΁ @@zp15%$/!@ znuåF)AI8 Hrt @x}cu!8Y6bII8{sg@ @I I$ŋ{۩WޗC@f% dĬ.P/Y3ī!$QN4ӄ7;8@ @HHE !@  ,A@$ p3lN @ ;~u 0 @ {Hه!@ $#o0m C] @  I*!@ @@,/,f%ę6B P/+O%$' ]Ǹ o;sܭٹC0l|h:@`Q;-*4 Joz~ߴ&ɣ+:ʼnWoI^ @`&H3ڜ37@ 5UCc?%6 D,؁ @ Pyu@+ @@YC 9$䈩 @ &$ z @r$ɵsOdFt Da @@7  FI"*' '_^y#i @ȒOeN-rcO!@ `m=&#Id[4.b @@$1  @@Uznu, QULib I,64l^ݰoⲃ P1WW@@ $(1Ge 0?;ߙ߉%C< D,؁ @ {=-c3@ vy7vE7AL@IbTQ$G_tpҩbKC @]$ @ ~\ Ig܊%Y$Ě$#}@ @`a$p @"گr6@K!$HN@ E6sYJV ލm^ox$RP&r!u__@(W}L"$Ao@4]ua J \!K>-ĂO!@ @@=5WJ\JK @ex}cu,;x ISZ@ @(DAE@ @$s$}# P$H @ @@(.d8 @ @D Q  @d%˻{ŠC`q$r @ H=x=gt@o@ @u^`HUh#$A5/=G_2('@ @ #$nGvp̍ @9ڹ'V<5G3},C!$C2 @ D&$( 0ŧW+oAAm0 @ VHt !UgV @ ,L!@$YS $1e @ w}]c I$ {۩Wޗg@ i 7??:N[3A+H@ @f $1t @&&pac;ML CI.t=Dd'A dK=ﲊD^ 6zQHK>: @`rmnZ'+CL*@$ .B`$mL0!C @ IgE@ ;{_O&* @ $D`1 @ @`0m^74N@$ʌ^C}kN< R @ $fN @Ho=aI!03$@xO>{M9 @ y I˟!GeGF@@dO3O (DYۉ9yݠS(  ITPG]П A @@52@ @Dq @ \ I*KZ+< @2"$Q0p @ Hˉ5- @"pgG^Eڹ'X; \6 "$1y@}q3\y4  @DI"H@ @ 0Z @!s^@ }kNŜ>zն\IdC @ $HcA.Gv @ H`cCUC`2H񽃳՝On/lC @"$ᅉB(@a@ @ sH @ B]H A9d$&CME @ .lW2G@H b @KCEIo=s# @Xn]B36cVړ}' Id\*ß1B @ȀDA (Ap @X$eś%#55@ؼ;eB@0$`tX#? Q@ [-q=ʶޠCH&@ @l I,\MKD O{&Ō7? @ : IGZ1b(| @W}i@C3\9KS:!@ $" @?waP@-$j$D@%"Y  @@v>v DUOI \|zܽ?@ @ I&|m{v, @  I3<·l3x@@/;9qb i ILÙZr'>G @@c1z &$Qpp= 8S @ @ :$H1@ @@?$~F']Ǯ:{s @ pH 43\f : @ ~7Ba,;' mȧs& @HL]Wy@$q& "pնx")(b @@L6VۗΜOd- $"6 >UF@ l|h󺮖RC;i I, F[]:56A@͋׷ :i}̈́%?Lq>f$$1#|@dL _ P>O{"|BÌzbIK$ږSҦ.@ @JHUFy8w_*>^Gz}vj @| Ήs\!te!QM@[@1 <QsbZ @`rĐ3( @@ =C @K;ϾƮ6t$ұ2NekեS >"T4"$ᅉB ! gE/4 N@v}9v')=hH?#va8  @IINlA @EbӍ"" I% @ {b7aD'$)!}kb @$fO @@?ɇ>} c I%K#0ږ7$WbN}i=B m5BIbT/4_  @17??a EIb>NnTb@ EIb.;38 fv!@8w5SgB.!D2 @ @ @fwo;?؏>/OI@&$H`v;?Z") @ @D!>_$G_ܲvh@@rǯ~R^$}P[ vI[|쩹lQHp 0_?2G/Wo cSh$RPf H! %LtXRhr I,7k^ @XdfBIH15q @FIbaCX6T5EًO=VVS@ 09m^7yTI IL 0.b HS @ 0(|\=(!WR@J!q#o0AA?+JB @ȝw}]{=f_璉Q@@tvsܭbpvl1{p I EJ|m/ mK|8eױ|A? I, |xzuF.:8@8nz*"A8 @76x@ wӍ9%$/n>9fD!@Vy%HtjQ$zbIK%pնxP(7x@f pwOz S%@K8rS@ @u@3 ZM(i @ `@K@$lQR e'd<1#$$g\  @vC@>HŔA Ew.42ka$LMIbj@ @ )<VqD!$#F!G!@M@V8yݲ IT`ŧW[  @  I|@4l % @:Μ8wku͢A &$Qpp}b/=gR @ ;*RG2 @XZi/ @#(nf<^$ŜC  V^o^-5Ey@dGxd8 @'$Q~ i @ @@H C {o;zv8vh*ßWA @s@:uB @OIb] @#p{i}f I! @ tk@d:8HK2m,}kNMYj @p@,/|mB ,'=f 4MHI@ @ $fNxO>{M9 @ | I < @ w}]qla I$Ca@ @s8[ @;$ zRqdQVJ 7͋׷ -$lCc {۩1u.C D$P~ P7$K"8@@ @@3@ @L`cC > P$B uڥS=W\|z5c ,8wBZ[(  ITDPO=ߒOn3ޒ{  @ @EDJ{۩WWIch @ ICZ@  %$ r ׶o?,g@ 0 m1 Uj@0$`t@ @@8$pv  d  @r#$[D @`v3C_p;Z]qc;'@ $D`17?7\ٺo)c0 @ Dlث3\~-V^zӽ^SnC/> Ys. C @>$|(Q a:@ JIbVT $R r\ @ &49lA# IǩȀN @  '$1g@ @ 0h YT @9x o=Gh DL؂ @ȁ39ȁO>@ @T?R]/orՁ{I<} @& jUnڼ%;-x]͗?q%B_(|V+fje ' 7{o;ᅠ>=vQW役,Z%A/ͽ^~7ß[:v%іkǑO/Zmݷ&[`ss\9(w%FfuRT56Isy6hGZBL#`-r\""KP7?[ͭ)s& z [frį~sѯ']76/>.B'*Gnv ~?2O;6bC?;gW{_Hy]#?u\]eA畈 dZg{=|c.#WNeݵ?o#%!$ۗ(P$A<#IX#DNCHLř'1쏈Z$ 1"?foiPOcZi\ ElW4J]md$Ѫ>ڑC%ea$*@4)Ij^I”!CdVIBSUFrĒ$q:$(UIbq_48H:d$$8 E`#_Z'qePڑSiK=0mJ 23H+&D(d I5GXJYzJ҃QЅΡJt?x^\IShV*IXIє$6% D8'LIDLI#T%I'K³CR 3@SHbܜf0qcNh)Dd] XMT3 ԏ Gz'Fdҁy|6Rf$%@4*I4^E_w!D(Z 3z%`Q.$q8d$UoS}GLIbZ$| RFKD"%I$A:$ w*ۈ%a !jBeF?"Iȯ:QI7l$4z$z!C%PkOWe)iY@W\WphVܒD$e$ћ7[%Ϧ$1oD\QY$"jf]f=#K1AI8Ҋ: xJx¥ZkI+$rhG@/h-)I(1)Z% 3!pShB8"Ӕ$5$ai(e-I4)3(VIe.Z+bܴzI7+ᖅ3n!{%bǍ;'B`jH8ԝ 4/i!?ɤI泶&$C窬 ^aExIB)#bzKPb9#$5l(I4JKSh% :QKU7>K‡y$abN$J6Lp(r$? K$IU0[7$Z SPGz%G)Kz|T$qy$#m&CN9s!D! 6 i #*1BDRD A^DW@ZOADDqPXnk;"7D2V/J_$BRW4A]f>Q%^)kMݥEhLPAeZiB(rDP08K֌_rB$DtP?>XzϹjPEA("L8 :vßG\#q BH\W"*Ĉ=U;0a[JD\1"M Q" 8ٔ'b3%tG\%IrJm"-FxbJDo[@M* ڕ^L4[\1$B LDHUbdr00+$fND\%BE$њ{mJi#2y)I*#tZ1aJ0q7$415kClĜݚ! *iYN)'kd +%̉Ч7גphf@4+$z6.-c$VZ$F?}$VAeJCWL顳FH顫"Kz%i9VuJ>6AIUU!BƘy38r39wC, y$s(yjD,xv,(ͅeJRfakIHELܐZ|$$ LH@gIHHr"y_dIl4zׇAXҔ!bV-M Y-IX[ DWg$VAlTJPLI"U$"J]uuU1 !֒{@ _$$vEH츑g (IBU)I(1SXD aF)I2DoE3K_:)I I?{*Rl2EZNP_U!F%!F˂j7weUHykNGč**%!n4l[KBL53)%!F4 ږ֒ε$||esmK֒~EEIIBdF̎-@:C62B1hM4EV"zwS+MtVDuшӠ #ǟ>7ʪ97jq-I86%Itmf$:U}hҔ$/Z5KVgLU”!L :є$kI\b ֒E`$!N, kd>/%dI'$O*Ġ$BrCגSj+J_KBo(e- 0% JtpڹeਫK;8$ȸo&JOy59tk2 4$4tYRD`4@ Ywj8"$l@^KB]ұkIۅNDWʃ:4{BN$(nJ;8./G\82'<fb>6**Fg21wjC㋩]?/=?Z:$ZnH  +KZKBNTG$%U_K^N’$&-I>wVSt)JF!0t2OEF" 5,̵$.ljU6pʧS0ZL ]K¬ ,-cZڬAXZښ)CXZFZڈ9s9 uV"~rKPbT~$ qCR$$ ˙24LMX`7  ̒T(I_~S͵$+GhG66lmOS9`3koʲV ̰0-_ɒx.(1VK2C0$<+R$za%@MI;: I­AXK N$2[!L ]YM%B*IXb*lJږ$DP#Iߓ9 xJzPS/| Iɻ2@ Ҽ9_CػΤ{'nţPQ%[c֒04KO%޿+7% ьH$1h * $fӆJJ%nf<%DWJqgsH͉t8$ w*.IS0-7% ´`I%BeJzFkބ$DhMPI I8z A`~]x}A$ѺS#k!6@䠬g$1_@`5qC07V$~_) ݻl47]KB۵3 I)C$I2cEA[5 Ib_U]A ͭ$FUz%#$"Ui$ٮVIbѼXxY)I JDaբ% ka&$a.($|}$Z"`\QB%M I !0$ =x- $y.BВR^KBy!zD$a$Ĉ֩, 3!M$!L]RYvK4I"K۩D$fKMI"iER%I$fu/ϩM墷oPLܠ{@ _H:6HvS<2 \ 3KZQ⋵$&I4՚$L̒Jh֔$.mJHZ !I%Av@%ID :up(jEԤ&VnZ $6s+|֒fM_R):]$;#@1HpO$ L% #wP8qFABG$dQ _-l$ IzU{),q$\.!bXj5}CW:Bl4Ajt1lW ZK"l濖~oZK % )#% `I‡ ?LpADd%IW)Io!{% u.:{i$ѵ%;n(ĕ$26uTqg- I!6Qv [[j/@aM]K5Y4:w ZҴ;#XID3"D[>T $T*$¸$| zonI' .IbvRo$ћ U!Lˎ7n/7)`J\6whnC+ISy>ѡ  P$qT$ć?aϚ@wx=v嬑;n IdqN7,_/I8v/IjeI"x)InC+IH+znIb.`!IĭHU*If)ID w/S$14 -I ZZwnC*I3!湲F/)0tǍvbDOܐlꃝ2FtyKIb^N@ cvh]Wk M@}ֹtK2I!IkVXP ²%IDJI]Ւ$R $B0[%JA9Z7 0;:_[ȍ@$!MPI4&AKܚrqk dI>\:vq qޒ$o!6MIb%I'Aꦺ!2f< 2/"tIu?mӂZV aiVEjV'=w0qCV LHq$ C`NZR\[{$ 3YR?{fe kM $ &Fv⎒$|$|"ڟec4%h)ɠ܅!jxUI"bhƛ"KbĘ^ƕyJ$bF2kkJ]5Dj/ZH$F~%~𝽭ѕt0MW,%EdnKDDTyD3r8=^fk"Ɣ7PHD tUĵR$T֬M7V!)zϡW}.Ib! B JXkI$ǤH ZCGMbfttdCz{{)R$Z%bu I,\L?` `v !@)zXm ZzD'&V=B@n=bJ>AhRrGR%JX ;-2ȝ@עJ 0MCaHD\)*bT S$ HՄ! I"A%!@ OS$zz^+;cLGqos{2QBӾ?^rHs]e \|z=gn$CJ ܍欍%Jxv)_ТzDYg.tIɖh6?U"8.手!$t!@6g^[h 4L"+_*1(EU"%;%'H(N)E@(:|8@@2Qb=bhe- =Br8 ٦8GꝲaCD@B V$onUX9p:uB|^Gߔf9YR2kC}7Eg SDhS'j6)gS#/$&N}0ԡjRT@@gI(IB K’$, :(Q",KUΒpѕ%:\ Gqo%G$Qߛ%%Di7K""ÑYz, %ѫGeIlteIN;(W5Kʽ;Qʒs{%Ȓ"I~U[@I϶w|, f{)+o&mW6M5UI* &  IW>7z% _$R$*$ϨͲ$񪄧$!uy t >D,K=?L#s$ѻĕ{*є$\*%I4`T=FRnFS2֒hnwE ZGnrh򒱟9S3dL^CS^:W} hKON%m=E ^2W/,ǦH%2~h|rN|l嶣zB3?C;Xkw5}ptxXUcT0Nr 5D=k#Jn#O0K=}c.}R$zD [LoQ1 ύ5}#æ5]$ԊuDq֊ɭJCǍC1]h:&M%*BhVM U+zjaJN,eY9>#< P q#,ibPD U"zDV%89H"=btK] Dk)1j&#q\=Q"`7欍A>DAԥ^=BycNpu)L J:m*>4+DyqvFXl&5Hju]u*FX,@ [S$i7 הTt)MQ*FH:e#&E*Q".+kljK-ח%qeOsOmAwdIHam>Yb,%!XV+Y:U3KB Z%=a͒hb ZrgIV,:(KBK1k?KBI"礤)IB}@_dFȏޘ@nGc1ُ@*录!;S9Ҝ[bTX?yD骘ZBB6N%壮ى:K$5#F*,}xs&ى:QSfua(u$M]KHX 3`>/KL&y/oWڥ9L#IS$RKzGGIBT%$ 9$OAx/x,澽$ĸ㹼H=B$<{r^I"L$!][s7-Y޲GJ$➝9$HII JglJtП6gphI”!ZW"ԒDaВD똶IMKy$D,-I8Y$ 'jI¡wv#-I mIJI5XtJZx2˷R@I:yfoq2iJxjRoHe8]"!4גY% +Q%GHP=BlIr*,I}$1RgFJb8-/]4Ws=^#I#ę^I§%`=B*uK)bshҕ(%赪SJ^NBȯHobXȂ7N,z"N3%0RVYD $c$ U I8$)[v(L$z&$0IK@D 3K½׆5qI@2&a47yOp̲~)&n Iz L I.l3qCoMpdI J, %!禖$zU Ib|1>KB\kIۀ[]'$ыW"!n$RKcqޑ%Q$qXT7KIe?J2qctOp1!}c&n Iv?=%@0$􈑒D*12Kb$E(I dIn+( >QpctK$EP5Kb$0qCuAYJ%uE#I UUUEIplKȜaMM$<w̨teI MJ#Wb$Feh_^%KbeIȕ% 5 ͰkǍz4+K":SG,1z؜QrzOe#I)kIĺƱ=qCQ[nԧ7̳Բ%-27Dڄ]KZtZO*+B7NQKE]5w:Kصㆻu" bu ˲`}UejMIo^#2i^P$qLOUhm).oP#!]Dp3qC.U"$!tDD=B+ILT.!!M+I8veI#āVI"z~"IB7U$ qJ%!$ 9QZp/$!4גK? Pc5m8F6J{p4k.&q=b _u@IG4ڈ롲kd\.)Fam2t4*ƺ2 0G߼~c,LyHHZĔ5#sL]%!c3 qzKJˏ))3ADBݿdC9.be'gܒQR$Z]G͓}"RBX{7?>WxwE$!-ƌ]GM1!}RU"LMJ#t`E >s(FmNj"p\%-1` KI: ??IHg_,U%:xD)z }7EYczJIEs[HF_չ"Gp1@T4D`=)$U%061$ۑ42R-e!t2Q" T >gC+t|TiR$#Eg%LЮj^HbP($QDϟqL6čG&R%b)T`Eѧ|X 9RJ8.͉@H8T)*ЩU DtċW(+EBqcC OوxU"'c|[?sz"Id\ "(a!2u`M(14n *WHJ|􈖯Rg@SȜ@ږ>w=1nzD\JV%#|TB7jlxޣV%7X='EKp(1: L bDzDHӫM,!|KLtbpDt$ofp qّi{ Can F`:9GCKy@`Ype \Ӛ|%!;tfM#ѠCיr\_x6ī,9(V-?UIu֧Ne&#U@7"~}@|RJ$ ϮXa1IB%qsfk%J4$2[$Ġ$Ȓr;HA{.IW ʒPݱK$zgJ>$HJ`J`fI8:)I"!, ^gIXdI6=DQT6) ɒ#II"!ɒӕ$ѪGHCQS$֏؏%Hcu4JDkze9ʬ #It/Xh i#LpͶ $, -gxJqY{$a]E$*C$ -CHaKh$qP+,IBd)D -I!ڄ$17Տ!0$!N=Kp$`ShfIW%pKW mDDIG pKd**IV'֚D!gIDo$΀$1X.IBr"*Л7,ƤHHB M%R!sȉ>7),I4.SĠ IxKf$\T=Hp6׾$H赯 J<+ܒ; ]# f$ѕ *IAK"Z$ 3-*% SPe$IP kMI_iJȒsKbR%,Iw^IBhMIsn-)JdShJ>ՉDFsO% 楳$6hfIHtYYp~Էa̕c- Z'KB2#TRxPa.?!{FSRkIRUN*IX{Al$ѕ S9VJM] 8!IQZvIMoԚĠ^%Iu`w]$1RhV%1eVIeGI!rHa%I(j%IEX279w, b9)I:qC5^, &, eT%RHRJX!F$!-U’$<;$:GTcDI W%JRV] F̒h]R%Ot%e!IhBE$e9u!L$aB8ċ^IG0% wDouHoCoT$b 2%躀^%IEUڵelVԞf'$1{p $ĄlA$K2Ku&dIH1$9b#$1H $ S0% Ĉ)I j]$a4ג #KB]>5HJYr\+KBkIBޫM7kIߪ8 hIBYT\KB%Gt KЛkXMILhmM$+C#A)Ih}S)@4=גD,YEkn55E"q< h(:~,oܮ` PJ& HIJG3H rp%).s# ]?*2 @7ꛆXx,Y X|i FI -[7:'Bj{j:QkLXyjQd*2]Bsff*cm_-fjɮ$ ,V .#9ZWq`mKJ.C,YuBt%aN&f5fIu-&I OZג:.7`>.%&%$fr $CEIbQZc9RR'=R'5<>rMBh0E PkS]v-":;Jp [{F`!%ZrVZR[}h=QInasysuzg`[/=}CiZCXM`$o~w߽wn0\ٙeϿ߾\d΅}]^,I1BIbYT:+ qU@"4Bѿ_#Rf$fqƨ8XBH1y?% 09S'bNќ}3_8[,C`oQWb#$q/|IQ.3 w 0ښ`4ΨxpRMzF=\@30ܼ+/ϦPHn)n#j,% ډ*&!P9t;,2|-.dO0|!zёZB$a?Ge=tH0O%fS 6 9 }cq>9{@!i)BfGmi iooZ#I\Bl\}z!P"k%%F !Y4u$YA&qL B}B@!!ͿՒ}@Xri{!B D\cq. CD$lö)i/.%-?~qv!I8܆@;Qz`# >⢖ _ GgjF3Hmnܾ$3`S`KIR@&$>;$ˡbk 7%C+5$tl *۰31p&Y|zzԘ-fنǒkɟ%_?E>#I@;Fř LK@.mdH1Flj@lsS]IbLX9@j&3 K`$:߸, ^4 ǣ,#CÍ$1!0ӱ]@=F<Ȓaq;onR&jI"F@=b݂ؕ5|" / Be^e9 _i׸LpWoG>NA( % Y$ Ӌ~>eC2Z{q͐$!0`kFI#x/)&nxUib5/><ƛ oD3͑$zQ 0^M7'ӨN9E_ I@. 0[Bic/k GIp%#* f(I I͸z$@ND WJ) D'$)!"_Ban< (Iqu+ EH6!tDnƟj 0 64 @s- +Ėv@H7|Tfή-@{`7 @ԩZW%ꝯ[AVD _)-́7X%$A߀@'FztR)̟Ljk½`qnV?+J."UL2-I49[ `;^yţ:z&D $nN=j8~ oE/Ecx"du o1yR;DùJf - $ %LMLf]a1Pʲr(%$,ÂSs`7uLE,vk'`I%M0&o_YOir"T!$+V"@rDQY_tl_RA-(@YJ Ic22℗8o$5‘?2Yh"PJDQ goQqb,_# ׋R%@ I4<"(e1qCmmE("L8L1kyВvK=kaI"L>a _"L$fO@?F?z*.@SPkaI" _e~ {xW$-s$ߊs >MHUbBTU$L6)b`nۣX$Z qۇ1JB!.~kǺa'"NB  D_˕$,g|zg"Oq"-B%F2LRԣRS}@+ҜD3۵Qqy`o>+7$P9 |`5d(VV˧7jIBn Ht aȔ XD޳=GDTy17/.x%K#2 U?$ S4$av~\@B+@:լs{ $E N*Ihӕ!@q ,]1(nhn 4pQ8.ܺώ6n %Ϟ/LjmђDnAI#(DvQᦱQpD2x(aUz$@L2߶3,W17Z!@d}(QM\; $P Hˍ"2ޟۋ I|-"y>ir|˟WP16{'4%VaIbP'd:W&@+KɐB%<2e4lYX1 l}֒Q0|#sL>jY) @`&dI~բG,5{F˵$| %dId ܀ P+niaZ$_lMYL¸qJ'_zc$$v `Z_okԘ!I|cs} 0&%E9TG˿6( %ZNˆlBPb(WHHSM$SPdL@5$XՏSI-|@~w6 IIbh )h$@.gHk$BV9 _ Co|#eYDkP3IM("”I$2"^,!M`!u "/]=yAFSLI 9nh2"FP K`!cu=HՇx2>uC K N!ILM Puœ,EXD bu.QW<&IQࠖsrb@Zo.m~֑$8Qʛ# oT pXrs$0|]rG d2\OL$Fyqc,.4#pSs$2y>+;xNz}H⮻2FuǗA :nёbIH' @z4NY2$%G?r]D9,Hˌ;`N7@&$2ŻDLIbNԝ1|F 17*ۤ9y_fD&( Fe!0+n ⟧r$y^FD ח¯3|:'dOI"e ÉCȝ#?$D `-!P$b:iHL P)$Y9KK2$aPp)$tla"L#!73!gI@FW Tx_[ji I,1"I@O$n @p :#!gAVU@ (dI,* % t$h @g~D$k6d@a#I Ei@sg:7z߾L$I Jr1CIWF;Li I"@] 07%@(!J9h!h *$M 6$({'1v-(X  Ib|`PLp%VSr|Yj P.F#*!$QI ii8S N[M gIaRݏfB27 y I˿$-|@$/ ʿ(%LO!@  @D D-%SB}$K"A!9?_Bj%Gr/d7vtl tSqߨUq֪c4@` *8wb_PVGâՓPknO$@Qp1C#X<ˍ 䩺i`%Bk)so$JFIgIJu",!-hbfId]E1}jd:{;5:R B`.JFl}*Q):LRP& 07AJ)%In%I0|-#՟%!U]%K?.Dh7ZCc (7#꿵D>PA-*{ Qy7@ u<6$]Xy 0A(*Ih!ӕD'!L0n$ӻ$"dI//j$)ϸF&QFV΅& }T11$5cNiJM5ㆦzoT L 'TOR1z0+Ws6#J2lP  D`>B`bIa?|cm:IB_L5eC6:8<@ Ka#CLBQ&nX2: 0ngD  'P5I$ K=x;wB)ik&$!15@ pk,T#M@_>$q$DbR/ZT xtE-^Sq+.̿ej!v|% Zrק`\&$S0۸IɟrUL@ nP Nm $xc΄'̡9:CgU(L,`t@'`vAVaI+ *M@q9ۇ$1'} AF!I_XKr@Nw|A-gvsZ_hB <͆@U$cT513|:UB`$J>X:`K+I0|M~6 _gCOR 0$ĎN!Pnb3Hr;5  XCjj}"ITY߆(Kr@e]~PppX0*G JBXօCMk!P/fvΖ!II!0z4x@TM! % p7[xH|$D`1 r 0z/7vx^ $R"O#@ iلG$%-9S' F"9{Q@ "nkabJ@'LF d*ZȼToݻ )\)~ZYr&xp$D$-"LL!I0z]|AM[SȒ<>wj̗%Q]lF.9Bv+AwIENDB`PK ! Pppppt/media/image10.pngPNG  IHDR|4 pHYs  ~pIDATxydWq'K 3~ . R>$o= #H[ z=z9(0z#qH$VP=/7S'=|zgݨF|~zgi$Үz>YOVzM),7oK sO~!sFnz:|z6|^vI@= nUOǚth1\iWTYZpr "Pow߻5ŀգ1zXzϞJc1_Oޡzݳh2zxw%>‡TĝeWOCHCwEzi{ʋVzT=T'3*> {.gW 4?gng$i%6'_>S#ˑGcIzt{\5G|zrzGz=ĝȲk§7O'գk}ɋSգ!|L= _sԃ3cQz c۔Ϻa#D1rﴵ) mIz=q`D|=:/wXz_R^%=w`.ǒsgiz|xlr/ԋDTؑbg hۿ}㖞'GuN=>*z'cϾ 4|< P/٧N^a&ID=/׵W$ϩi{ z/>i Ez> hY粨 ){H}6k|t K%Z,9>U=\:ԋD:QO~x@zz>9v[-%ݸ ezL:P/UOVs hQ>\J_| >c7_~QЃ[Az>\ }k> W>vvTS˩vaC$Գ zcIt4& .OP>O.F`dQ" ׽zo(؍\}z_sGc̩Q$|R= Skzg!cûw~wǝ,z3+gwz OUsG3Ëzt0ٯ^2g-6rCH {Ճ8QO-z!|L=79wE"P/Wz3mnا,—+K˴֣@Hdv|*|rʞ'qhskSWo˕{z/{,rPgS rS8KջLy'f[%7O .R!t? SBrA#|>z>3=ܐՓ1h#7w8 "M"|z_U}wz >5syR TO/DUj79w,lо⬽|xzF5|tTo7^+0Eٵ'k[Ͷϸ~.ǰAzԣ+ULqEßpHJm>zjѧ!a7ccj'?P/XbԫSO}0.zCM]J$|8de`y81>e-=FTO}#Q'{Օzr.kwezqokW&3QQrѢАԓFsWϞg'=GEKG‡c2Ea;İOT>_OSO-ڂ`eyhmҏa??FV]aQ'7 |^"ч*<[X~aslt\k6K_|j#S喇#Aww /YE+VP*|'ΥPc]/7٧W^WJu .Xd}.o}/GâOUx|>9y?eETP/2}?0>jv{KsGFuPQzȃPR'] $@G}p:Go{36]ni$^u7xU:gC‡z˚.i7H zcE]'ɣ>lSSɻzԾ sBr%TT{ZlW/eGm'Wox)@Ā*z *zvkm> C֋zʈǓ_=ceѵ7K*|r$u.Dz8ŠaWC=۾0.zzPDOO\A |I$2ȳ#|>M ?wnq+pj7ت;(hQ,yH.գ!|Fö32z$ofSkzprzeGzS٧_x9hL2zj^N@PG "3BG=-gWC=jc%^~=|RGU=f_%I|ErHZ=Zsl@"V48Vk+}tҭQ5Wo8R9k_|I$&>U@=< 'f,#ON띁Bz6|T=LtU_Uؤ˒soMrU"otم^uYEFCo`_=|FJ^6;H=99T6ySK?9WY-lmŖ0z9j6Nag uꔜz<叩gUR tP Nƫ^k]= [#z/:HP}|EX+<*NfVOwuβH=>Ub~ȣO3w'* zVضmy/ԋIq ,VW{rl"yrFwUO:-I " TUhK?9k/ɫճG3F:$ԋ<7~ÚHS='+n8~,ME܅%Ez%ه-D^UXg5Ylj`uکW^K#2بz,؇VR9ygkt I޴ -ȌtG+޺6Ηa-ܨs>kz' "#SPU VpԫD{ɛ.DZرg=/{59 -E_2]S;lA"|uv8J#w9Rs4W.*S˩gyuw-0drmBH'QT< G*z-7p:d큼!$oٗƱZ=jgty>j*yRSMW%]{^dyg_ujXG!Ϟ@gܠzS\zwꥑH0Nx׺\;>PE>ǿ̮T kOKzMy[\VGlNܹN/ufܣ6)p?>8g⼼1W|R/Wz۶V|`ʟ,}8挵 IQEq6opqP/2Bه1=wHĀG֊ZPn^Xzi$R/9rIԍzk]Tԫd}nS_Lo,ٶUcᏤ=wLQ|<|=,z1wjn&[S{m$yuSTBoz^dсH_ũZyԓysx͝>իJ^zńz>KScؗ/G^sz^"iy~蚷 "=z~G_MQ? |y*|5fkHI77_T %ϣ^zR> T4EU؁T%WPOG(i2!R`;BȳC7[iPVk^ Szo,qF%πQ/=kO~mOzgcv~IZnGrO T_L\Ƣ^s6&oCO~[.D<ɓ{y"|XϚפ,6o6m_X=9戓zy|zHGWz-7N=BH߅^Cy6|"|XJUҠXڢ^d g'Sɫ_ՅG^ޠN "}Wuk J^ND{S%5ZigP/-y]!}uO`9 j9z9r J*վnd?[C=?|s8HbQ5 < _%zXxEp3^<[z띴^[ iաzǜ+a.[6Bs޽)U|쌋J1rk~Xr`w^[mXtGqGyÎ^H{zz7otPzcϺ}_Q=_Gӡ"yTaEiA=f_Y.DRTpR<>'$WnRCPӯpb_z>_BŸZUϷQɣa(vj'_2Ϳ3IQ^Ϲ yE+ tR̵u|G3{kh'p?zϪQo<ᣥ.xZH ;Qsek}Nӗ aҷ\^_uyw8#@jbGɃz 'jEcS(;ԳS1ԋT)%kz9*]pRSCr/g}l=W/>Tπ\3m&8C=T/DPN'ɫ\k]z:ݾ[rW,>}}:e^u/ݛSJ| _ĎԳʓz*|Skzh^<_NAkcGO]P>SUBH Iyϩ\wxvAwE_K*ݳXyÇ۫^yr^XkWUOySoj~)n I|T=akObGݚz^d|1roQjptS*5oy9zZt{ }]( z/{SM-zRPq?,wQr-vT3oLan\Qg\opG?sY^/䗝OcI՟=4_?迫4\K|l lgd6z !H|d?UNU='|K_bÇZM:uz+wL= SGkҩ7Bz*|9@"|Hzq׸왧У)ʸ]͐t{g1zgꍦX[艨xFO~K !yEz*|S!϶eUIGcQTM:mWD: ݏ|l[oSVL=>I^%r Pb}g~N=vnAUOOCj%/>[d5vT:ށOp;U=JTT/^>I^z VZ^]} {0aI}yS= SӺ8O9[.m 9Tyz$/_N= _%w@/nGrQ$y9Qݜz >< 7BO-qhxSSգٵFzw5g^i>V|U =mz>z6y >zC+R/ۊ^>[=u(o_--7y*I^h g{R\S.or1E"y>z1a\ph-Ty*˩)ʈ]uW>f;H1rOUr7GkUMb:-;n@_Àz9'c૴va'f*WmTrIrъTG0ؖs ?b&{RrV=/]"/OOo1ȣ g×^-)Ez sҼ5zu_ Iom~|*ykOuמuE$yG۶!|F#POzc1Գ7BO#eGcрz|^4ԫqw1@y+}L %ìR>FT/Uπ&.TsZ8r}CNOG˩g\"ݏ9y'~j1><6WvnJ< OlIN. x |z>XYaP=o5%K0m%k>WO-G1hKIGI|zAôN^؅zM1<>&]z9P=_ZRԆ=fzv^FjC <3L#6cg=!DZY>eT=燏1cjzV7SA.OnLԣQtR|5ka\;PZg\=jb3x'S[҅y|MԓoG}G-i$OJ^yE{󎒗?'6vߢnOZ{3 _|@U>[=?|һz8\'kl{X0`}9QzVN%seN>:9YY/N$|w ㌵Bsk,2 'ʙ̂8_|${lz6|9`Nϸ}R=?y4?W>xR;5^ 3Sc{3wT='|=zUMˍcxzf/8WfG߻/_ڧ\,pգgȓ[Y%ǾzbSH$vR=|}!ON*|PcM_<ũ'Բ9y3A~[~t֔tݿ5gWTvn ˜ԣ% TφOyY[e[Q`N~:I=\T3ޮ1R!|$3; G{zԾ֪**Ѫeg6ȓs0v&O+!Q Y/5y6 B=$,H;U=j_ˑWi\[J/_?xI0?aS+՜s^4ryz9<snJMs+O%Wu îu<9+Ρ\Qq 'WmH@=,rIX=zF癛NGWTN=IUYxy\j5c૭][yTw 6G-;w<UIwz>J]癎GIz>z7L'aU[IT c+S'ltr:b+L=<ه1Yo7;z b^ [!O.O$W$իtF[9G{y$5NC:^+^A؆G=IRt†S&=!D`8y[Rl };C= pnJ?E_nɵ}쇇k^y' ƜtmǶ5Գ3ȫZnΥmL-qТ.yz2yTO P497r&=u b6|4oeb o߄I^9$yz Lo~[9[R(`;p<[NƩIK6I~=:J4kZԓ]!SOb'ճkeG<+c.K4gz-"w\5P K:9m @=3ggXQO)JT Wv;<ˍW8ɫ^[ CڤfPQz@ϘǼLGzF>\M޴+Gy: TrԫPG#π(Qq[]ww5FyPO%}V2>y-#I}9"JC]T>Ì嚽z:j8SS+\niK9z.4oՓ%A]=/umK0$|̾\Ǧ6r/7Ka)pUn3LT')yzpJ9I0W<{ț}Vm<,y6هQdԳmS̘c9ɣTRh^y Ӎoވ1Fa2G^U&)k)GSGlUT$|w>EF*|^%P~=?yL=?[?yM tɹwmV3Ƿ^իvqьA7;3NZHihT6#O=ɬ-zP?,\MgE- ^U 7gXLzعdH'+T6K yuTxgl݁.vIf؞c\6Ӷg}CdG Jс]UJUBI wE$y9BzΎVchBoyUױg /ӷ-E‡@z>`ke{R|v vN5`?S#OgL\z3+֕HS!]̓'Cro>BTϾEwKuRqWM^א7oz >֪-g+9/W6rCHkGSSS1(|S!kr걢O^8K~Kuo+೽ɣy={W{> ygGS˩Wl2zӺTCzq; d8SzkS?(5vQy+v49$g?;7bɀNSwJĨιeyJ_zbw;MjW=,_3ЦԫD^m 7yE;L]yk̼SǦQҥj>@Iy;Wz#r r]X赣^z~ $G+uQ֣#OGG9Tk"vEWЃ/|cIhGGla7BTP% o>t[G0!Ut |h]WQc%]ٰnO|WH+g8ᝄkO> Z'su-So L|(Dɻ5j S/m}UKC|$r9gy9FA^YZ Q1h j_Ks-)CT;-g4lz+!yص೗&|5{7C$]~;rI?ۑ7NHS=@vG,[Փ -.X"5E VKK܎Q>CaVGՓ]3CP=]׊s}=劾JQ7 =}7s9'کl #CgLvQ(|W\t`6=UIoZ'9} |Kw¡,ΗskS$.j($5\9 =PO_J>S!yR= ;O^+垡m'_; ~YSϰoZvکnP >c!3yգ1\ˁ͓1rk^z,9T Νj+8:rggM$7bD%3A3C\u"arz@n9lX7ӧWD>' :Uow`_^_[=ְn-3cl_mzzl4t+MU"?-0so:oww<>JSφ(ZwPTn_|=I:8KC5roAR| 穟(V=yZ\P%π'TRɣ}xM 3^< _2Z5 xi^:h+^aW>'ynUՓSwPYZWY=, T<[=[)'ɣA'IDeٰFa_%jV˫EUocԾ N/P+mF;&Kl) 93೗}r9O83uz|Ǣ^w<zl/V܀,9~rF.bߪMwr۶:fzꝿێz?e;$ϣǗgfPG$Ok On .7B/㾽]װxc𱾼"y>_҂zl¯G7"|UyzBUSᣗkz}U%I5~oCUy+'zyIUZSRg_y/N+TC=?տz5 =Wy;3,yzƖ [9Go'v|*yOZGՓW+o4ݺ/GKlk[Hի׼=ƧYUE嘓I}&VR=tn?arUC$yL=?yzrVo~vuM.CewೇC"y> \}˩}>OTU:z?}6rkuUeA;-u ?'IUGsz ^ [mG_+JwիM^ f<9|1!@zEoē1@JQyyL=ˑGWG}m٧w|-x<^^7]ڶ#!ϫx9$|9[ڞM]-P=C>WC=x^e=eZ>xk?aru^[ -@1̽ka!|v ^;GQqsa{!yC'/e Hgwz?Q^^ꍚ<`ꑷp~0OޒVy9*ucLTp<ޥǬ $8A}n^U$yT=ʟ:Bv &yhX2Wz :zx8mzHS/aU5rәrzeQ=P1 (yoFmஏNn\-[kyk]7f hoym1NqhFN=CU P*zH1pA=J1pZcSWTدcQwz\?l g WY={uUC@=0V/GzHZ9ՃY_U9>==ص'cQT|}`srˊ+׎<:UPO𱡌?bs g'[=jn ;ܘzjPcԯ'k^^no,&yѵ5RXq<< crC&;b9GK|<g'/+O;7alؤ\qgDm+*y9Ƣ^Bo*uTkI[= Ƙ=!ჍS1;U=/G]Yo?ȃM1 FȭΣ+yiEճkXi7drr#ԻvKh1l>_t?ܶ ԃG3;Kh swүX5TBO+l<ۻy[Vin>gQc걢ONC$y~}9ZTO׮zn9jyU7{#]˖cEM/ש3~kZɓѢA EJ<WCȳ=PO—lxQ/woϿcLNêQw[ϔMݻeNī.,&/O`Fo, %/}wj'գ- :ʽQoJ 9rh}Jw@?|L[_|~1+1䩅"x%`Sj!ݘzH/^JQ$yNy&$w*|CtF2KI;3F3I=QQrUUׯc]r>Cmw"e8Htycvn Wu4l xT=>U=7$CjSg'k=) F 9h WUScru!|E$|ꩣ] N;'t}+GcIlwR=1 !|6yK4*7 _goMԓ]ڵ)?,=1{ϦbճP%*zݝ\¸ˑ'cz8oYgfx1 <ᰆT'{@=L޻ RϘq7nLnX_[g%;jDMM=Vecz۷1rC?# ToZK|uٮ=$*h cF Uc x[XypφvP=krMMY=杭?ݾ#j#7An" Wv W]40.Uc(yL=J^ܫ^n+gͩMQF zڭi;]˥/ӨySz_Q= 7Vb %'ɣȳ#OҌ;C=IS#;=;뺎 Zf hP*yT>ȃx U<ڗ#/z >c͛\ր/G!KɓVma׼@k{8g#=Sr 1B=[-\x<-"yz 5osrVI=ؾ3y'Ec$|w~딼 +bI= `#OU䢔գs(yj#ww+@&F$|tT=Yۆ˭wh:zF]gz3lU 1l$y9>J^Q=\?zr Gճcﰡ2Qt{[s, ^~SkVgw{"¯4Q Tr-ܪ\//!c# %x5o%(|zj VOoz_zIO mrކQUw\g*c!|pTTSQh{n)9$|0$<t̔QոPoEv4>|t2mim[=$գe/]i z{Ov"|x…kh<}9TYx:tH 0T_s!2t:΀Ϩ~~ŅF.ϩړa) >Too>QJبz} q']X֝xyMΑmv7ly`F6WC$|jTy˕{HN<W|*P;vf'SYnz$ٗ#OI[yYCR^|C<@Uаգ}^>JGO˥z6|h1T L}yy'd;p6te>[=JSt?fV?U=L=ȳcQ}2hbWTggx#OxYkS޼&>o~jz}T=vi]FTȓ1$|%{1;TՁ/=*|~ҿ|!睓< 9k<Q s.3Q$y9(|Tz9X;wūԫ%Pz'Փ.Ryw*|v!7 o=y9Wt7lTZOgǚk Ӡ+.z~ VOygg{G; aN84enzXzz9r]{x7Ԁ^U),6y=$‡9cπOW(+짓a'%k x(Gl{V? /ʾ"|lN1 [y%;0Thq' €}Ea)gٲ|1yt\Mq/WucٳOp{[@!m<>cVHO~*2ݤ^wbjT\ɒw4L=UzAv̩]u'.ŰwϏޱk>2GzcaP=cßMlh3L1b{o-XQGr|`]%$FG- =/fx{wHFIJxYk&0F῾5w//m|jJ^zt}6| Q.^.MD|>C=wr:8+C,<.#ڻ|b؇ӕzs E{ꩻWI]g-2ݒFC=>&4cATTw GRa[h+,gX3ٞ%|~r+0\2|hk>nmaeu" |sCJNG;u);$xEl闃o.oV+J~w"cgW8E,}p G s#wǝ΀OUGJ,z$|NI_=>U=izNbGWQwq ~{#(SvMFrcgrv1z >3:lm{W@YqoAU%0 '>~%JIrQ(|i KxbySԣIԳeC!N%vǷw^[Fr%/{J?xsW1r!| ճSɳ=؆IvH]QrMء^a^~bD<j_|sԳ=OUOzU2+z}tJ qKI=zI>3CT[(dOxGZq"|xJHPg{c*TXĕ? _mrm_E9<->ЍN-G=|zFo%d}|rw4cWdt9rU Ta{פ>(ez_[fǼ3Wy^$ԛо{QĕжWFI{r1 ބz^ԻHgz>vo̫N,ԓ垤Hlݽ°1y^T >Eؑ_[=޼enm2G zxKQ֎x :U= Ov} ) >U=gwnǤoځm?~6z鞿ܳ%cQҍKߨP/-w(4>ɓ,.C3ȓ&-GC P/R= _ =~+>C= _!|HN!yEBqǺ< _N=>ƟMPgԓ[XToaة"޸ԃ㹆zHεcգ/@%Op,bx2H^ؼ#]׷z̴ɓ}M)ǥz09Hkf}s2Uȃz{OcF#OoaAlr0Qd >\ITZ`=*ڱoT3=z;z?k'S[m~q5c^$zj#sldQTP=Y9Cy\qǾ͑E;6w^y}K7nz:PM%3cmڅk(|Tأ";"~hѧZ>I^>J =t =PBة"-t"գ^&mI 9L=9"ԋzcUȃ}9U=#π/ _qMUO{^$zg\ӣ^3c1UR^mM^p#mwվH!|H.C^TR~ɓIr]Q='EG6;^T͋"L=P.9G^mE(|]m>P/Hkf}gpUT/ncܤzN(| ]UXEB1En;e ]T]=gw% xVTn9[QPodI= #0c9Փnq.1o{gG_ "Lk|ԣQT*K,JёP/yU"]V գsߊ;>\5T/HkfG1>l<1i+T=P/2"+WcǾܩWI=vچO叞ta{S^>V/_DhFZ>6/z_0d^U99$/gxKV{|{Tc3ނckQ=վJ"y9; ٻE "?Ez[ʽJꩧ9|xM!!/7eWO=xNi،7>cy.6VTIԸ$Szזz W҈* _=X,URhY G=fLYJzxQܩ;HwET bvl[НzWU=;Hb'ɫ^c׆zPoL=Vs72ze<z5{UT|\UR[iآzl^ F^ |t֞\KSCzwZ_~B`+w^;ntSfpzQOpzwls#GG3#/<;Uc3ނ<^orP/7S<& r/ԋ FkotTS+zv󻔱J٧K.zSE:6/HM=L?grn,zעzci،kr-t#oTO'cfzNK23˩ "Ի5ь/:+A=L=٪B=$/ԋz#$|H^`Zg-H7v!m|zJ-7]ⵥ,j "-W\{C0UOYezFT 3&'@#{Tc3ނ`u~a\z}vMa XEBTcQX<%^Uc~qvO T)y^zԻHT=;zH^Q=f_Q=w^Tx V/͵skWrϯ^z?w[ g EBRܘi8Ii.(|E\J kCN:ju l֎ͫ8$Fj&øR=SWTs^[H^>SO G~`zRyCKɓQlM)l S "]QdBH^* Ɖ*w/^&F¤'c0N[I4DܽƁ's+I= ¥N<1m9zr,ֳK_)GF g$ zxz@Ćԃz_+Z旅=A^d9w >Tg\AS詯 (y6|<>D9$*%z^{MTU59E Z΅zDBzr@.TO=I^>FO> 炨kcȓQ쭨^Ty>k)sP3c5T[XzV5韾z&Gɋ}#oѵsum>< ͝>6Vbz>q^MgTbz>v Ly?V̙x:jœs|R=,H|`lʑ)4ߵx (O%ՋO?2#olQ9;!/7sɳCIl蟹|>ȰPocQ9N(|.z墯8\U~>E޾z~ysxF<>IqzN,23^bDfY7 _Iݓ3~c$B*䔹B={C$ V= /Y~tK,ݩSߒkyxS+%"x~|G]cwrTnin;>hykdV<+-t>v{8q]$.|}CċD"-mD= \xDWA("Ȕիm_U»H$Rƛn3`sz["3wͷK4H$2LӋ}z]$zErwHz![zD%H$M7:}ݠ^m$cwHd[o@xDzH$E"Hsn5DOnH$Y> "HD"^$zH$E"ջ[#Hdd[oD"P/zH$E"ȌwD"P/zH$2A+!Ru IENDB`PK !Gddppt/media/image11.pngPNG  IHDRم pHYs  ~IDATxg\u6i" `f*Z%$+gJ$%&1$ &I$$fR@V-Ke%+ͭ{>OZ{:I=v]5}(xxxx.Ǩ|>g{xxxx p1=<<kV't1=ixS b dיrA)47=V b6,KA\oifh{0Y@s߰9f1&4|h;WEc[ DSb.=<& r3W6&zh}1US*,(.ew1=<<VBLД\Mz򋇀fmoy1mِxץ1w?` McxbkHi&ļdWo!S13/2X.Gbnyf&9O΀ˍ̶b1MSS]+Kh"\HhNqs<<<, +%8Ǵh"͌~ټcL3w-)l /eiQӢ޴C Ͷb~iKṭC Ht(e&ThFf5b_3%ft4b "i|.KiZbc@R&#̧\T4;i75ۊi2?ǶF3K>K.9E4i&*Y[WݰXܢّ*Du9אbB״@SČhBG" 1mF3㘵) ,%&oeVO%!8cܹꣳ9;"\_ŴhI3M1kw^R>l-̤, )\b5Sl+I3_O~_P[yms}yߦ84Vox|d9 gD5u1& 1.ZŠ_K1j+fP}0Ds=<<$K)x5\s3i$\J*Lq)bs4ALAS6DWqh;̑`qWpu}si664j)h2h{C3)hBL2s+!k211:OKmK:񷊇ޜVLM_ qA#3L'>115Q4!&c.(43/i!?M.G帼+5"oZ~ fvf[1Cb"#MhÇ ?AL)b 41brSb4okiKb"S;mVřfTLK! 4!ٷMKF3o)f )hSMb*4U;\LJąK%4%9PblbM)h&9&("iTnft[{e61w1=pC.损˿Zys!Z×%jL.o h&)h*1N97vYL7^>d؈Xއw|u-)i1%xfhoQMF3ձ A(4Is+S_}j`DĴ4ecf5z;c1S1Sh쒫4d1[h>:#GS4֎M4S& b y1 혿dW޹:hNs=<axsF&o[94ͷȟY&b[ ڡlӢ) h*+%p$]g!p'd7ˈASd4Ag44? 1QE6c1Uhs+fQ`=hW nfJu `%hM)1˧v;8f2)^Y^Tǜ'*c>r1>c1Gc7Sfrϩy3M%&,5IBP"ʹs >FMr VV b2)hfcnJ3y`#|۾T\1<<yseeMqSfzS"fiؕ`1-:5%4S}f~g/xxdL֘9BThBLv;ieMb E BocnŌG#*mj4S-nW2hŴhBk;tL3YLAs~!{rr%;X4ߴj.GC;6DM~Z1B9PM s!j/4s;-hg1OFL 1f},&%!&u}K]LJ 4yF@JL)\͒b^SHM* 14";rSxw:dNX7RϺ7X M%mŝ4YL$<TФL3fBT ;oh>v&Ĕʟ)ṭ|yx{/f@&Mv.Dk s4yGn4=Bv5T43v o6meGnb& 4Uyճz֣X&ćjww.E߱$!yxit1NWkK)|91iŔkN6ÚK+DR݌߳;%:e7]Žff%G'( 7y'pㅵmC>x9&74:뻘9+q%)&M;+ۢTs;{2 8X˜uu7#[avt;FwrE n<ϷԷP"it4oB!TbV1ܟ^NH33Af;c|]q5]u:Bn"o GCKM_9Cp9pp}bzxvh-vxl?*&IQTT@i1%,C:*Qvkt_q1!}KRhfPbдՓy4YV.NyG#AL妠224Ȉ 4h&&sNn4U%n֌]{qayR)xsLusݮӴ).eНy Fqy7Ag㛜iJڧm-PtҚ8 ׻Sْ7cO5ù|Qb|۾ \~cGfFV%UG_Hvm̅'ft bm.Ǣ2h&|{;>Xxw.BRvv7׉fa3_Y{{qM^NAYD.f]LqX71)\2TrK Fި8iELvSu!&дU/uJh?xqFSy"'qLٖK= Dj;8+ڪV /H.EA. :|d1BlAS.:Z 1Uiьrcɔ XoVYm}eӣ\Mi2<C̶懾SCo+&z\ܴwӾ鱸T͊ 4̏i7oAsp :m-+1vݩPif~y/FdхGr%8b[M.b"Tb L+4sbOȏ^Q1m%)\Ĵs4&iŌuS.],k>|4K%&wV 3\o$`ҢkY˶X,`.;sOzTK4%&LMj[5B1iZ1JԲhy~P^jhUf˨v:/lQ b1&uϿ.r3_ d̾ps ̻38KQ#V \:v drSVGˬz#E(f4X\IXKфVLU{ΫK&ɛtDWr^>k1K.ꌇ潖Z1qMi"LŌd7+e1Gx0,O_;cT8 kFֽ¥ Y7mN9Pyy4ys` 2i_y[KK*йTnھ9.TQP>rw#w1sI3]LZqH5y`b@ThvێŽGEAK,nﻘ}᯾lf%lu2h@3QSh l*.T:85l^,xLc4\Cj;! O^j(;\ܴbFMd!f~2[{x 9:]-pӊܴKۥ}g7 25ۇ-x4sP/]z ]Z/^=9t}MN9mvH}n]՞CL=hf.;25KCۦ.Ǣ. z']YDȚvGJʘlJʘತ7e)ϣ901X13/] .Ɲ\"#4߶w&n{8Ȣeq,PFm1L17nˈɕBvLzE>y1+;3h1\qB#Ip;Um])]5W_K|̧z 1%fn,CRGi (ZOz]9.a󁼒11{lG.߹>P>laMR–Q.ĬH| fێ1\~!u牓!ԝqyLK%_]Tbˎ|)иt1=cvi{O1e}LJ71ˢhfV_dB>]܎aK1=Hs =n>mާPZLc.O\~l YWa 4s 睏X1.%:\JƇr!]݊s+1ͨ<[{и̋MߔM혏TG^1SÚb9sTFeA#I3o/f_H;sEGO'xvJiM;u;1G1٥ Ąn2&:Xuzj2 @+U3ÌMcqѢDҞ?Vbʇv} wmt9Z@-:Db:hVˎ1G"X$m /(DT$"â\Z 4?WK3Xpъ4K}|ߣS.{"|xܖӴ%TD*=\B3:!Pf?*H銱/((5y4ߌrrhbvfV-qmh|;݉DJ;) UlU ETfj20Ŭ h}[.]LQri '^3G'E'H9:Aޡm|gmh9N.щܺ|*,Sy̓035c=ٟ.LjLɈv?m*3ZE { qU"b)+/(3 %BĔ+4Zwe ˌ@j9xI(3#eNG]1zxۦ$j8n_B嘩H'"ZaFjg vڧ1;?xj̤C>d4Shh{YFb]Qԯy~ܞzIT?sG=l{żãmDPNnu|M'#D>Bu:!#9^N}|M3+~ʧxFp B.gJLA3ofARJ̊%:% jXd4q* Fm_RЬ t"Djv|ќOR>riHkc*(|OW.-fjH0Ͽ{:K 7:nT1޸J1ˠPfG}&|*`T0Z"-jSz^mj.nEqefhYYj1"&ц].C{{ =sb2JFztUqt%u]BW51S:d@SmĔv腑tɥ=ZM-n>1븋<||G< Osȝ#jr3|۲)B>ĝVOAs\2'`1Mn&71;-T@Tn7PbRtk&b;qYP>sVLFvĎ%Ug/g4BdKDM_ "|_ VJO;>yS=Q(c^N0ѕ H>)&Jl}z:]}9c.2 JRd4K)b*4Sb2JLN6S)'sbFMv2]qÇH덏Oh7\QjY!` ]-Qewb &d4lFTbh^cy]>U],_S 4! 1%Xe1Sg3_a9 .IL&f~pOT'eڇ?{ŁN4((r2ғ:UíJS1&eJy#k8 bL}:6 P 4s5mW K%f߿NؐgC ͆eM4uWKȵ 1MO_8"/&T>ӗ!jF'F|:> F t#G_;꺦r2(^ 'K Ab mᐈKsaZ 1Si%WV _Aӊ>bM݌dд).ycU;"8=˟Ph@4m+Y*Nnsi{9N\Y޵@3#kJ6}* \Li@\ͨ*&)hSq'Ey=ŬLrGfBM)hBL^creFLA3U.CBefC8fjid4B1MJLG[c6'fTT{@2CV%v\M2/>: 4!\BLi\̔ZQjh6oGTbAhh/^:1MpYFLfq iV\r;Љ|QE/$ \J,&D s)2ʝJL`/NG%W[>䢢25BCۊT~9b~^?+SiŴ\ %S͕#ˉɝtS=LV-Z b*4յbǶ+Rb VI1Q"\_hf[if8=SuGOxOqgV"&z忹}.&z؃BC%&/]F.0R37g(Kp3fTLSXUǒ\yJnW>5#Q{VLӶVLF#sPtǮ,R)hr V k5u[b?Ŕ/ .vҊ 4|L7YI'sh2Ha6׉]T\hBLAh(>bGJ[Z cFLLSpD$39fJLΈiTXs$@c˫ 4!iTBX-NOէX).]qm3Ja6:!;攘b2JLFd4˔ M[.)o%Ĵ2n6.Ԅ;ߙH!룘&s 1Ѯxr'gbMO+!pG&\iЄwΩ.LUGLbFǴb4S eJ{G/yh/X'Wьvo.^]AL\BLn賋gfCp.SO/_S)1¾'\ ~yH32ÏW\J?JMXM1G럚n'Ϙ4ST^%{4Ryhk|LTvޫ̨e/0hFZftu}(%fte7UKh,W$2h hܳ6.cG咪W.b?NY͟|CD,q PJϼlG/N:I*Yͅ]K%DYFb V>D}zEE)Tu1GZ˔ft%sYn%ĔGo.Pj%qQѬ]rF0a%Z Ke҂}2˛l2SVKiR{fGۊpU1c@%ftEz볷OV1R \Eb%&:pV.bĵ홗&yohM:Ze"h-:XZݞӮLRnF{9d߼ vS3uz]E} .O0 Omd1fSFH3 @v6cQiwT@MTo!$(뷩vtƣ\VaŶlf7j-dt.;[ D=YJLLjK\tå=}o.bp Nds/c4M˥~u뜪:>;{0\6~wro޶2@1hʩev+`FQC.9mYόoG9gTR>DUK^p% /Z)h~Q4[n>JL\Ќ&?F̥D~8r\b+%N 2bM5RgKJFϜbbr&tR$UY!ٲPd.64јՏ@35D@bneFHBbr8{/b7YLAF ,*BȨb8fҮ h7?3)-Z17,\+]ϜRhH6_4e\4)e& WMK6蕫1Zo=>| T-s_(b̈́hhR-?ʩ:m?4 2 J=M\*4M&w̃ѯk$%b#cr;NKTs e DLpLx1͑Y%`#">1RR%f,Szf'bfj/9 Q&Mt?^UbuӚMi1f=*.m5(ɣi%|{;*.YLhFЊyQ} ·Pb2gQ>=*>3$1°Ҟ5?S^[H61&zpM> \m&_h v'<(d#St/DX7yGWKa.%RC(.UAj0 Y8hƷ(~~1Us4R\njK&XL9;L[9|.#1N)0昶.%uKN9ԧ%4s쌯7vyΔ_u7e&D)GeltiZ<*f -;@j&Ĭ~g%NHJemN)싞.f1v)ň6Lq)54 Jb2ф"\NjS1;x?Fd3P@Hz3j9i{}kCkB0\mth,yGbQҮt1KhΥSTɦ\}e{@n}=_Xug娸T9岌9ɋio(in:R\JMpb2uxLqzXىaMѴ#=؈Yp9zW#1fńJ S"ZpYidՊ 4>fmGZ:U]uG"f/;Bʏzmӂ&s 1K9nx.RwZ^2G;tbt򆊇 VLF3c*m<71Si&LTrh \VIw֭_nR,CqD +f>}b6Gbec.֭؄4E7h~21r.e"Պ 4˯urJFŴtJ;"/D'4kgݲQK<悔xο{Sͭ=ъ 4<8V^UK0Kt)eF-Rɟ89م#bb3:C߮QCȇVLFӊiuQ.G(fK%p)4y:h āeէۇq!]Ҋ9WupCV*T\Z1-"ftPIG%ik{uJ"UX7/D&s 1칠ZoT,=UΥjQ!;qe*+-"|x[FѼ0WMI\|ּy,ɥ1$~n22E?;kp}ڲm}"_}yWNRQ1!3 7MЄ^!j'1Ae x%Ou!䂕TbMK):vg\L '\=%!kݰ><9O#j] T}:KD2:Wrz'5GTլp=-NDMbIɍ3M^Nj* r)VfTLKg#>Z+3o*sb,p)4K Fd}zj~].RBbF5ܰaG_h^u ~-G a·OHHj%FS~L'mBO3nZડtFn3ӡhx#GIVE\);-S\dSЄ"dcV ps|,\"Чȷ%vֺ{cs@\g4a%BД \JMR_8'LwߢdOqstjr#2VedfhALFSJAǼpSivcbCXZ48.[۞̫b %i} fKNtaY Fe~z 8ʣXoo8|D*.WxFL̸9Z1{_: !j%<b@v3cU=%G*K>rQTnU=+f'_4AJn[ bM]g5%&EjJ̨WΏ2u6rWIMqkBXymqz_ӻ;׬|Ny; \[).OrӢM+G Q~;6).Dթψ܌v>=u)P4RՖ@SԧCLS:(fj2ťd.¥seRͮ$,ɣ kk%z.y{YL'pTbwH9%l۸8]TէwɥǜnE!(d^a{FePr;U3kMSjsY~?Mw;n2})tBpqzr[8|}zqIw}zϰL-Pt?'!Mg^"M̕.YLfmMqs\TF1濮1q+dSԝ33_1{LZ;2'MfFlYnT}zfdB'㽯ou1Ot@riŬ}?׉S\xp˨hALّ/POEJLj~65O0y`b y+$~Yy+ø8=_^rPE=K3ֆƥE-a.Y,-%Vpq+S_\ ʒDZ6dP2`\}&>5u6%w|rKR!hJHɐOD=n\J c h`2%fhZĔbbmG(جSc*AK'?,)%Yp\y vGUjʞ~A@fBbvڥhv\W7eMͷb~mK7E{ˀ!5~wa\L+m'%0jәK[>trVF ȝn .IYY{G^J] ѽ/C;yvA,Y۟Q !W^N2\Q7Uo, ̽O]!Q몟їD}rXB3U^$SbJ[rXnL37}osgͳw]n1eߡ1Ke1M;vWֆ O.!&K6'2mvweo!ˣJ^U% .3߷{n\_̶Q" ן}M-dB7P(s>+N}S/WLd?曍fvLw\J'rօ4SbHVW4S7}~N"u2K,&QYˮ]"O8]էGK~٠Yj;G_ǘ[3"JN.U9AgM4pslqy؛Y Jh4*}ځFC!RTDZ.e^w f.K.9b4ψoeiqׅQ1?[_(6׽r$f YAl<&ǑiY)~jnᳵ;ow[19_Z[+W_J.n~u-; hG,u7)1trVWh-F1MVd1S,bϺdI.Q\>1<"-Iuχ :)+%Xڧo=[&nZkL97|R7T|'s98ņ`H0.fv"OreJ(J}^dZ{t-(Q\|agҰIdFLW xt>9Z>K޴ m7e=.l %d.9n,4̠A[IJ0JV&pnBj`TQ̒񎾬U\FK\ ̟i̚oG~YCͫKD /F˶b{mcjU8άYQ1Uf(Å+1;ecm}웗IfF />r´/ìy񞓊Kc6vPP23~ی)h!XvJ.n\ʾT^~bTLF֋cSˑXߢ4"S~#߲Tq 1҄Ȳ!#&'|b*4eZYf-eJͨ%lIBk(a"Ww%;m%lL-0HyeO~|>[Vd4ޞ pȑz89pm.9.YLlMd<{t!Uv!fxw3H]stZiʒ\Z4@cb\r1? U b*.{]%;u뜄SZRbbVgHT b3νrՃbVEŌoTST̒{g ˎzbV `V~Y3^1? %f&d4SMY)\2x]I%f6.VvZ+{aN%٘B}W WFQ(s}21DBjY+p9 f#ϸ\JM 4-c;r@p{ B((bֲ˭߯ .^p:Ь ZLɦ:Gɏ98.;brpeE?#DCpЭ%d.^yG˥4pi.%M†龋9+{٬ӊ~f.Qmw،6&^ԑe'>ثceq'1'Bey9Rq%B.:yJ`6ILŔ̋9iCkn{ UՍ)e|?ú56]T~|6dZ|>S)9f*.7bs&E.M] C7)3}׿Z86vQ ShuQ>VobL= fb_s/ Lr7%2×΃.wE=e.DkQQ]LA3_2-|5!sVPbEjtX)__Miu/ExyFe_:Q:XXY˱N0kuƎncnԕE1e-p 1>eJd1o,ELFC˯l\1SnVʉ~ B싊i S\3bL#J-+keS\vc/\2j&# k9 \BL)\kJ~^SH0?_:,7}9rF PT*7HLܖ3ZQS5.$DO%&qٍ|#[Y]ߊ}|xuqއNR9&wK\o9e÷rp\@3U*,r}xC9+Wb Gej_Uʎ\ fO9f!U$7U)9&[T\J\+GNK1XLҊ咻ϯ_-bF}wbp&(& "ߙyX\*1s2XXظL̨W^ ](vH0@DaokfhsNxGb$OK4wQc14~JъR͈9P.^%Ҋ,>?cebScҊލ咫%n.$j%wͩ^y 2^޺e1|/t@h7RGVD ш>^}}ڃB\01)1S[ ˒5}I-E`<̍%##f|[X.kO>%b2V>qbM3N\s;˶bam><`8hdweFetnr,eY*oN31 k\(j*Y"%)hr8wׅD$))1/[#1]zBg>:[?1hxgq0?|YfҮ\lV.Z.bdh% r!+ZST|u M~=u&,rS^Fd"Ui[[1+Qꄟ )%SŸD #3kR\fJ{O-^YjFg1%#\62CŚ.ELJh*1̨\|!rkHi_2;CUS9OGE"g@eS<\1c1f떬teaW\>H0o5HN0mS:~UALDE6|A &~vfya͆ʯ~n? nŮS7{_O>VvnaArYgm4T.iOQ\/!&Ќ7~OJLii w~uSc.#VLd,;ΏuPݓR0 k]LvV"'y\.f+.ftPbn\>vTp)KNȊ]ֱڞ19C(1[$l&yl{}CA.SfŶb?OW=[x0̷}2`.v1^!\|,Nﰕ &1M1NEcʦGSd1(1%,l(ׄRAsb`'Bf| uo~t6_Xzp nʮw({ē`쳘.bZ7%r 4qX 4om_YZVLi vG%LuA^ fGDťh譞##-[\.0+;>o ˞ThbXv-C8j\>~W fj'X.J%fN,tյ=%OtD''ټOYF7Uڳ#3&__/[{|qYC6jQz|vzl͉7X1'4ӊ֣b2*{_CU 1MiK%#be̬DA!4T.+%0~$/e傴;.둗 1go1¥$-qTb*.?.%ꀊ_G013fwbnڧTY>!p!H&CRP.7eё֥/T+E=Roed(j%"skcN%"&Ќ#1Mi߹j[9`RĔL9/Ƈ+߸wxaط/tbS\2åEUӭ[+W<5#}ɬ9'le\iY\SL\rE. 3qN|dtъ8FO M|`Dw͏cvZ.+=lݱgo1i>#\BLܲurMqT &TCKsLɋ1M!x쒹 >L%E3#&z,L6iS”5W{GtlL(|ϞT<&%Tn6,rٲu 8 %"8%lEv1f?~6$1v\BuhLT+;Zv k͙A=?JQbBMD }l%/\łb*+YLFӮQ fhcv+& NSL^]D] ʶ,&b_54SDUT{;[*ЕևP损 1Q?ZMzXd+-=:#׸.e\V:,r/s,bngҦ̥1{%ft7ЎxR6d1qTL$p3ʥ;*&ꑤ$IT *\įO+b f}'5)Nv\*1ۿ6D&V*1e/b̶墵2c×G]W2n ;ouTL͢i'q «| W蕫Si s--0ag> b".EYh;z1.,ɥ[_.kC1&zCu7+鏞"n)'\\:ro[sO^=G1Mcrb۟0.KcSQ4SbF)x(4YaT 1Kr`d"*3?'Ou3\.r+Eq=vLa3KHqi7ThS%,&)bL.Y s%/M7__!ԃ=1W4@b*:br08&r䨽VB}5ׄ u4ՀK 4b w)*L5>t\o}sN39vщ]uXHs 7b[Bys'^mҊy %ĄY+YLQ~y[9sr1Q13\^9y-+%Tpb,b~9'.b "|@LA{V3%rF~2bp+9M`1s,fɟh*1\([\DK5 1[Փ?`2)1;j%W &)hWb6{LNhJLۀ&HyˣCU%VL2*fW.\fĬ5’Y;yb+p4觘Wir)zST\.x"e1y$O ے9e1?)h*.NX2Ֆ)cڕ[+.=.\'2Ui`Ңɏip=s981/1j˖M1MJFSS[a!Nn1ՈTbbhb=#G˙QUSqb4ﴱ%d4wcm BUHLr\^џgAu[I%1.U{~t|(3:ZFL *)pbJoJ~Z E3>sjcb f\d}KS1.!s}kB.>FleFM#./Vw.,Бq"@ ._hbdnAsW@.1-CE)1cZ%JYχ sa1K9ƕk+K2\FD.ELXb6wz}F3Jrbp}6e,f_qAb=K&)hTK&KSд;r~V~~.V\J'"& \JLv"&0ՒsvmIr4O}29A}Z1qF4MrWZ52%&?j{gVLr\q?y.[M_8.?w9wL%VLAӮ-m%]E[1fRŽڞ8d4i&МPb5F1YF͉¥-3?1S6Q:y&JJȇ <XWr0ĴhQ,&J0mC =Ua@ٳ }XlsȊ,+.Q//h8#[}9fEkrXX6=vq״D[#¥VKдJ2Jp}MCLGݣ .,@;.9}dhXVSЌSєl/4y&%.E[DLϊ 3b օc4bMH-0tV&0_KF\2^w.|!=A%'1 (abGbrUV*11;Yu1^9i'+ˈɽrn |Fr#7KKi ;F#>ϼy5L/\ݴ&zc.).VN hF71SnS61fr)v(ʹ\BLޏ;2)/h݂'$_Wطל=c4!?:'ˎgN+4U^inO~5GG3ϒs (fN/Ց̊ 1p L|^<-!>uz,~yXb.9s|}Ɯ~-QKFT&Β#d+eu?Ji)`%K *|J`8mNbL3:(ZfGRJȭQ.?w4ЄW|FmGEv 4%"o$>sYTGW>*V*.7X\~q_'wHbO<-Ka|{ALA~߁&1NZ9C s0bWfFOe%ŌLjr9WQc<*:ا&9 9UI׭Ft79A |c/IpzAnr{ pbb"yRѴb"TՓs.Ί)VJ kASg4ѭig~C? 4sށ٣bP Dx֭E*amm%)k'oB[k'nBŐIͭPg4SQ.-KSR?L2ǚsO<*&nfJ鈕' fT̓wZQl}L%Ⱦ7i|ᥜ_b^1\Ŵ]r'tLjsLobMЃV2 iZd4?9%xzͺ)49?ՈSoZO6@x5SZ85,e+Y1zR扲m'f%{  ؟\N1:Br:ye#6 *Ӭ y1*EL (2+l[p\8n?|򔠩`]%3&)hFUKӦ5S|XLnSk%`V 1m!bryiHy[F;Q1UfS@L*7si좉Rqi牓"ĸ5'1y4Ti& "W0!&SYLCd4SqbNr 1wTz5FцpR-JL%Ԙ46{~on)BOK5ЕH}6=6l*dTVQZYPe4 =-GNg̒xc.X8FS)kd7Sb>DeH3H¼9̒LU1޻,?iM3Ϡyo37Sќ0'4f4sd{VHDqM.Uy+yಶgs;nYE7U VJA̶bJČzbVb}+ eLӊYEe 9=dɹSqb*4m SJe~{ϓ&yRD(%% Je[^&1'RI(X19d1"Ɲ14VZNbb]:)\ͨB,f.`f2G,RJf.aeaRYPme!%KC4D[wp+^PnWp)b~nh8XTr>\ c@ra+:[8\,M-pl 7sSbFPyGVwM%[bAsVB2>pʊ6TEa婑Jᒛ߳ĹWONoSL1*̈VL4ӑO(HM:qi{1fd8 P+%V)+y2G|/h3@;I%C +q2XiD- 1vۗ"y-4Io[͔۪A/TmCMHHDX4'hryt/8+&U:ː(1qK J3z=8KK5-<ևFg}%}t+]l2o,fdfN1DFLޥu7\jSz)FnZ1ߛ۶Yd+%\M ܊3.C-ͼlrّK߻\7,~4|1όc6#u.?U[&9?]JY̫X3)h \hh3MFӺ)JV盅)67~㯚r+.Mfھy²v`wbMke|QQLTbL9u~-Nz6\lj@\Mg_bb4ќhn9b5}s@iTx-th`O@ӭ!pE=:Rp|_gԧL;i([oYLrLN39Yx2Ky 4k%=gMPJ^bH*x6+_P1^.m1WR]v*w1KhwO)h4JbMӢ֢s\K,3&脛k3EL>KfT-PMSb=FM%ײZ +y+RhԙS}\#{~dŬ\ϽM3 sYe'|2@sⲵ.5&j&|l&"Ε}ҢJz:I^%MI^@4 ܫi~j 7Dy'`s#%&TU}l{(4siMm%D.Ck˧9v=v6̈y#Db6$1fܢJ)+\DC_NDsOL1BS8f=~=~̦pm4qjtحW5'Ԟp1\Ήd1&66ƶN}FBou!j[[Z[~|Qb-1of7N4LM٨#4>wɴD5pSĔ"we~sY )hJÙALF9&/<}:W'>^_QC}S0|1 ۚq72Z(\~">i&g߽ѧ.$a@L% z sgT̖\LщyfaA,&u+@S1-bs&h?a6D:K+&ڹ :%<`Tb@3 4qT&5wmd1EF ޭpgVLX .%̥󙋦ELAs>!.b :\Hd1MfR_Ā&c4GNDnmN"\Y`2@V*.p'Cؽ>19lf(1JLcSY)wͅr)!hm&DMkB~3>~vSm(Ϲd0ҳ bT\;i$u!xG/M_:Wb 2 ֖K<ןQ _%&Оٍ!x@V _rM ˒bn'~<=waFFL*I+ψ 4ooCt5Q:(*s"&VLnD()D৭XZd V"p?l>]TVI0ELI3]L,ԕaJ2/&_HS$m|)b-sزẄ́%1IGp1W\ڕHGThUc4VIMJ)3 p1=D7 \*1u)1iKJ-b-Z V[KFS+1u.Lj߿m1̈3;UGđm}dpmbHPKyxGtSD+1Sɬ.g+e#K2{_T|S0B1ALO8!?&g-f^LF3Z"),=׺-rDK?@۾!̈Y39?p1'kOk+WbJՍh:q%Ĕ Ւu &}]LӣK1aa1q^ڻH%\3.%YLv3ڸb]iT]rc޿_caŹ* &Ќ֕z̨pDr.*ftcv? IDATMl_1y_QC}S0Z1Tb#|d9saBLSm!Vcj\t1=Wy 3V?MKUMG ~|?͔OMt0ٳW?kJ$;3Z+o M>#1ˮ mc,ɓ߽fdD]9p 13'4xJJ0!?v÷%{,1E1?ufO,bWIѮLob %"SELi'T1n(sq|2+黜fڂddLIc1U'!f4U2"&QDyˈMW{ 3XLnSUe͑Q1 y1U ߦɟrE1?#%{5,y婺r;]h&[i;S\Y+:3b1 &wSSQ1M+ݏczFwEOLr|Gm.3br)\\yp1gH3QW+ObZ.qBP(Қ*T\S,#ZrbEb.SДvWsJLJ.&SډZbz{=zL5/- bBIꏳsYRL2*ӊos_QC}S0*1|K̝@g~`VŜHJS+K. 4Q)\Z1e?6no.x(VI %.P\%v0/f G \n(yÌhZ?wbYcfdZ1ΥT?"ӊ#-:,bw{WP߿\-g4wtRWbhi혳j<&[ 򷴎nڥEQ1q$>h*:q;?u\LS0 JLcv$fk<1Uj؝Ѧz̥[FLf^L1=\6b;,]t О8c^9w\9Č̘eL$OG9&Ǽ~VQ1S jn޿䜳=Fk%cӦ"%y1118#~7hf1S I.?F3ʒb~ی4 %1N_sbЮ9hO$EomVs+/XTѤ="%fk&[~]gW_.u1=F$׌EԸ5ѭބK 49T1M>ю0"T;`f {3ѸR.~rV\f M [Oι] b.Wؼy;M3{|k1&jZ^g.iu3Uj\2 &"JLASkZ\^=\̅%ĔDLՔ$YH ?JLfGЌNms ͙+U373 v:\LѼ/Gc)b{seI11T:HRhٞ"v (p)4&TbrLٮ3Ms4<ťmL_u.%{M T]򼘂3z2G̈́@S-u]pًy;Vsb&L!IJO( b dž5ief5{TLu$hDhqy?-q1=\E󼓘1q^yhTbC-WIUI))VX)\t %&o'ѿU7mZ*x}'x")1YmzGuSGw'9NnZ\TI]̪|_//l1Q:GGLD=A\F \Z1]bm2Miˌ@STh6mNbbM-])m1&4U{6u.='断\–hhꠞON{oT̟Vo__…._ߣo*s)h j=m@fL$bͽ-_b~#p 1bv`.f1&7XE MU(\)b 8,vފiGuߒoZV.()&|T甥е*T͏D3,=ft1M{^p$SM}SDu'p1]YRLd|0}YJ\f.bs GM&G}bz^bUtSTb219hh1{jctW\!#tCV2ZSJ~J06cy{bF=:sѹkJ27u%=*u%#'#y<<}MT8t(|g_u&uϨѾ|q}馺ѭ6싛 =𨐘xn"ѭ𨖘mCI1k7;ӭcl>7>xb#!.-&ٶ}pÂӰrbQZͼn")1JQymfΚ{V^1n:!n>ǵXYcE "[bpsbޭp1=<<<\L,k=<<<<Ķzxxxx p1=<<sRGB pHYs  ~IDATx^w-}X,!pIt^rI(d*W")V7$z!  IH{(%Y(vf%q*%@U_'}{Lٳgf=3׬zwp|g9r8^    fq4@@@@ >@@@@-IlC"      lAHb @@@$@@@@` D[@    @$=   [ :D@hhi݆m[Ͷq\W5pCC'R˦r-#]zbmoI26ձr?"Zۻ|umk|5mX Qo -y4P7qiӷEr`=?f"٥55rtaRKOG$ ZZ~ۉ}ȾbU~'iRZ޳>Q^Z /kJ%zU_+OvGump 퉚Zw`+o/;qշ!hZf8-w7?+*o³.I7pͿnKWqߵ_fkdž[rߑ Fo?5X?ԴZ$#+@.7b7V?># 0@nKR\ /aW˿a?k6|3_;giFׁ6n[y)E\tWloGLo:&|H_ p)goÆ 7͑q57|fgmئ~q[k6-R涫_L󟽕G!X Bu|/ٯs闓P쳟ZHZi>m\B3iZ1ms-l2Zګ x-ޖz˥糒VelCϦE I Ƕ  ,0߭>-|F[4+c6gd][z@}Z|7Ҧ\`y+]C &/!=JTt/U/gϧi`o(Zdl-َD @!zT6=mHkoYܲ-wc\uzk%y=IGĪS^V79{f$ ,un 3D{5@j^ olI8y8ΆXYsvn[ d>nZx>m^>2v3bHb[w E@-ru_ӿ%4l5,}U~sHsjZm/[֧[6_z}fg!r ]{FB.)[6J*}޺! :gKSҫ^ws4 [:6v [@@@@`7E@@@@` D[@    @$=   [ :D@@@" @@@؂9$   Ip  0C=;ZuƣpT֬y#H*io>scۥ'L{l.=uT̗&\ۯJ+?lANtrú̉ rvt+ZXf!O"έɆ ZH[Uvu6 :h3KYxot?M'`8ș;ah&^z[ròl:VgwӬcض1NH[! 0=(GxkjA.r' C5E+ݜ P)b+; L] K>cgZnjwt{XIf t^RMм;b S[WDP\Py0;3%IWE6JYuS;/7e?~Y# r> }gNE|D[ @7|ɴlH:5b%q6JвV࿏[-O[׽总̶݌ذG!q@ب@s'-Wk/Tn>vݶ;+/v-*'|?ĝPoi9  \mnДmW4\G\zWݶj铭r: hM2wm8{"@ "-[S>;$XJ  D[V}jʾڝMM*QybܼovIf;6g҃j鞛Wեfi8ᴉ^{Viۥ5)5ߺUeu8b|\NX*y߅m_aCn+ԵWi滫J2i_̪-Uٷ#u@@ h \   P%1 &B@@@TIb=@)ׯ&-՚."@Y?vW"h"uO@@@X"@ n@@@؂9$   Ip    $!@@@@H{@@@ @$t   <{@@-]#G6`y{Xz&8bK׬Q}&tv   }oyɃ ;iyVKϰl|!5vGtvc   k2 u%K׬Hta~TD$]#  SW_KMR=x뾽/mK󶕧ڰ|dzҾ!*@@1ܯ*KOWNT=2*-K.5/0nP@@@`Q}gv=u$AO nVD>   -S6U>cZkmf ۰?k.jl@@@`(A_'+KT"~Vngh=#v[0 K=[a%o+mi@s%A$a    7V&c@@@/@$ߐ=    D+    o@@@@`e"@@@ I7d    dl   $@@@XYHbe26@@@@9/@@@8p@~]癞Ri73 7 ;A@@@`0;z/ 6WAǍ!   0k6qC"6J   (\wbIry[nynYq-    .=&:KErcHbu7ZFTIb   tHKj0 g+WNm.e]Id@@@6'P {5;{E4aH     0jp\= GIth&6A@@@`DN :u N%+qc%.VF@@@`?}I9JD󿓸B@@@0D6JBX">zl   (It8t'FpТI@@@#@D=E@@@D @@@Ic[@@@(@$@@@@D=E@@@D @@@Ic[@@@(@$@@@@D=E@@@D @@@Ic[@@@(@$@@@@D=E@@@D @@@Ic[@@@(@$@@@@D=E@@@D @@@Ic[@@@(@$@@@@D=E@@@D @@@Ic[@@@(@$@@@@D=E@@@D @@@Ic[@@@(@$@@@@D=E@@@D @@@Ic[@@@(@$@@@@D=E@@@D @@@Ic[@@@(@$@@@@D=E@@@D @@@Ic[@@@(@$@@@@D=E@@@D @@@Ic[@@@(@$@@@@D=E@@@D @@@Ic[@@@(@$@@@@D=E@@@D @@@Ic[@@@(@$@@@@D=E@@@D @@@8rH]8p@% k׋  LdžsP^?:e u{׃   M)P7hp-b5@@@`D3n\.-HsgO}c%G{{={~4 gqtO~1o8 @@ƒؑl4$:LǏ_X~KLs~}?B_x1L]<]q(v6@@="kݸ _OAsO,P0wG<3:$ .1oѿ lb7nR@@] ;`f_D(&Е!f E ˎ M8x[B;ߙJuHMC}"  H$FF_#~Q'Jʇ`5W Ň8&(-soX||,戀 u d}G@@ IM8U"Dx7RǾt">В\HeDD.xcoz&J`B9} ~%y":L}cz@0ʭ   0v"W'PFxpYvEK%;_XYل"sn=%՛bw_TBLuL^*,S#eSbL ~)@@@`DhVp$ᙲKZѠ`"VH> ~G8p0]JES L8S LxOL8ЌgL e   LKHbZg0/wg~QOS0,n~g|P+~#W|$/eyS D0:j%z6R ! &r*=@@(@$E|V2B1w$:P*`"$.^8BD[?.xK?T͜ 4=Tz   0")c?Z gadUQqv El{5E𢇔GDq`BS/WfF#D]aDD}?ЌS$޶mWB+D=i@p  LUQS=q{ #K"PˎפK&׈^J""2P0" _w5O((O{7"fe w% $"l5#c$G(6bc ׇ  Jb7SovA~'xJkLVP6qk~Ik=i:hr0)R y_0D"EAg?k#DB%gXن!$qaDJI%C@@1 Iuf~nQ$BWaDzW}$MZx8) -w*J8p6q{iMز8gQb`K-i(Oc`lyDJe-   +@$1'{k%a3^M.'/Q<5S:Yn-?k%z6<7jkC%?5^G4tP}D WD Nq  UHb-3Vt<"{]C/{k #м M+xDh: %$F\#_neC%w<"E"=tGh+MO(  t Ldžo|&v#G]K$bͿNw|dOi6a#nzsuwcil HV/HDHr}6FQV"MGJ,kY@@6%@$);q1fĽ]xON%F<D '~i#rqDJP}׆ݐzFQ$\"qOƏF8 Xl  }$m.$.|o (rY^nj"A^%{r(#Bg9  *@Īb gOxMZe#DD$&<աוH(`80 %K$SR:_yDB"#<"ƏDP u6  lXHbs;Ǟ~v1lҰ C8pGĤw#Cq*fνm.8Yе #"(h'eÕzה/ad.Q"ღ%xǒ#ʯeC)tOҤۗ=   @{"V DsۏDJG'~p:i;뢳G)ꯑNމ ۡT"zmh.ӄBzmyD"^Y VBf]6 &J$<"ĺۚ#  @&@$-EYjK%> #=nﰦ1y#xRl&QQCEO~J$LhOt齍ݨ8“]6_X')(Vۯp!$T%y^   $6o>#:xT"1 #L(GN%R}<^%XY(c[V*64(0ꬡʞTceR 8C@@`g$v\x]})p<+#I ?Oդ)19VN%n9.緿F'jY"#.=k>C  s C+nG5E*"=XTDGx,PBqέE7-UO\(PmGl(ʈ ( ~r*0.ѣˆK$"PyDti;R["  @&@$-\@yl}DM!LM@:y/{&-T)gƒBr:nLmȶU%BG #< S#+   $|Y$"?˓W~:G<6w;&SZzPu><;khr %KɆX  ĮjۜGh_W'k:7)#{qiqĹ=>뻡^~^i>!3M75]"J$6"#ҧl{#.}CIH%4   @"hUG_zD0yQT9P(ByWGl2VC@@V Xk'V#U.?>YDO8K3/ Zr-5jfZyN4Ir|ޣTQ#\%Q#!Fص;E@@`D@!wHf "y?:^iTBaD0…K6eCNGdo}gZXLʕO<#Y>YR }TwR)&s  LA$J:G|եE^SD$"^ĶH8¯]ym??_'?=ŧ"%#¿=MU#ι`e**rHqCOw@gJ%L˃_2/G0GC1@@@I̥%{\G6F@9%9лF>R yR[Yc/Hxg r7MJhF0L(>yRF T<"^ꯡPb7׆  $6N>:xێ5?Ҕ %Fש'Gd(HǏ:J%4ic[pĭ{cIG2y+wzȗ_O,c  "Hݹ  L^-'߄}.@y+HBUE$a]_D/.fA+w8P>e2eQ4qkMلgaDs5S}ԏ\W<{P.mڅu@@@`RٮCZ?yDE*<qqc0Kpq4#VST*nQмk؈t/gCv;gB@@.>w6>cR w܈<"tVcI>"" &ұ$| #:ߎnS_Ob'я#\?D̊#\yR4Buܠ:"  @I͐.;j#W~'{Iu{D …^I(4 s/Ywc?wGG8x"lGͤyÈb]Fbl  qcn}quqDUD6J%4)(ui <"V@#zވTTB %`BLhR0Nc>Ԉr0BSϳes@@@@D;tdylDa}eؿ½6*_UZP#6s_zGa/^ƫCZEPrAfڔ   0K"Y6uW%xbz q](p<%.WGF0B60[lk  D$&p/ovQK<<0aIx_J(b_ߏx7ec$T…|Mp*R ;+r{& #\a _bKmIGl*?DT0yFͽ  LQHbV}#FxƑ_L# _?~_]kj&R Gyw|HB%|A5W+=wGU`^դi5Jb   0E")Z9?FDx/{EA WI8В$<ý6?K'҄[Gr*I [e<Z-λFx駤#itN@@ ILNՑDG8Hy޵\3#4HB4xF1X8fy2~D1[*"4|:#0BkP.Oi4=g  $$&LKNR<|At[U^DxF/KDE.^Y6GɞQOQ;Z T.9q#~ !^<"-áG    -9##tg-З4" =<-7|^gboD fςNio2L_#4xwn  c s:7GQ<"6s0ӎ_/< w-Lp*HF Zk+9awi1eTIdLxOGDmMpl9   xԄϝS?8W+pǍ#F8K1h4L2"?b) |]j5MhPi0lBaD9PqD>Y1 @@@`ZTIL9[#R0]6M(;% G\"-B yĘN'ULxaKG Q^.1n1   cJbl-(T|E|փ #<"vTB%"x"h uXl`|o&nxJO39"}VGyf 8#@@Ucl6~uo $ B ]%\+WJByD:Q1U1wUMcmw*I¢*k%$g8jiw!5GG@@`ZTILgy7}B]6 Қtͥ ##&yw+&P£Kxnuy8m@@#@xb3μ?0"6>Q-ʈ(P53~FYwUIhc<Bmut8(eZ")gZ$b񣪞¸1@@) IL|Q-zmDI_9t3_Bw WD{TF?z,ʎӺi.O# GՐG4ILl@@آ:$%Qk|7^w>!КiD:]Zn{9փ:,4OHnw?J%Ew]P QwЈy }{~ 4xY*GͻF]$JDߍK isdg   0g$F׺0#C[:{HKʈrD6lDȆDbt7@' ou* E03J3S6gj4v  $F~:kęE%Ń6⑟ǦCh軡nAOH+@#w#{y˺G6Y*A$1hӱ3@@f+@$1usT&1DPB[I8ttI6%$u7 z6Ixi0ǩL( "֌/ǴiЦcg  VHb\MH:s?Bi tw4KC7HB랸q CMIx+L">=>r/ z@@y 05#y;B8<"[8҅wk;K]94)H׌,.8;F@@y P%1vQאHh޵gYkXFx5"u6#F@@`VDhN:t$GDPB3'`1ee$Q8y(DIuRrO WCd, XKr@@#@$1 =T,"I]ۇϸ"F`TQN"ƒpU&"rMD+*M5)A@@`Do>xgIDH"N# "H;nhy~gP7e$i0FQQ.+(_7lX  $x~#/sv+ lH!T2(GxC7n7xͮtQq -$|"1e9:Ȃ n 6,B@@`D[n<۵# m|<{{zGt#|D#>Q VFqmG$fDz..2"~I-_=|л~nͶ-GC@@`D[n9H(u/+$DJ"$f:X0{{" H]0T!HDޝG#ڙ##  0%"mVt٨$GB 4bD4CKq c}7W9$$;'6c"  0IGMu#ÚDG]"8;к5p0nծFCyRNI1De#$“ bkM~{gKJU:d5=զ  LFHbkMBĹ{a=W:e$껑G萎pY9%_[=pD>r0C F$qsm98   NS#LT?hみGQ Mhllq,h&Iш+∈!")K$16,@@cIl\Ahc\ El0軡5-4hœLvpDPQ!UisN@@ IlID( D oY>r6u(D[hR/(ȒgFfKW&ZJ    "MGd[g  x$66H"#|6i*H"^#12E0 򈍶(/Y,zjD`ETfDmX@@,@$;!#m6HBy7'D5HE5$6c=RD>r0Z!̑eJ*/mw  $6$Q< /еHBp$$"R R LGDbsM;#eD9(wp>j2"  "  0" D6t쬣~J%4Gd zmluGH" &TB 5'⢣Qa<"   c PK# iOE #" Bx _7Ժ>L礧%zGs$5$"C$1{C@@`\Dj$mDuCDwPeSb~/:kx\l0KV~Ću ѽwWE]C:eg6@(nĤ ")#  c DK: $QI DkCR"1ILJD6UI:V"){OxO48|Mѕ#&!-q8e@@F'@$&)G(4 ~!OP"FޝG)gDJhx@(>"  0.ƒD{(Ф#)ë2“ GDb1f-xBЅue CHx Q{s̚C@@rG U.V?k%qAOp}R"޳*b"|DO.5vp  lSHbG_9䃝=4j`" BVp-:]{lKu8+ZODto~G8e@@&@$Fų6lTFZHD#4tH8{_؜uD9EZ1QId{!3wh~ӼG8k@@&@$FzWIy#T.OS_w,K# >e#;{9'k݁bl˷zmTJS8 GMhRF Mdzl?&@@XU*x'h{qÑE@Q$4rHk%Q$XI ߊcJ"rńr WFtܘ   0 $F :oeAx4HOX.Ku;Szj-+&I<^hs@@ P%1pe%7~gkCKJB% # 8}N+oAM8>['R\2"{G<BGl&]E   i"㱋껡$J=# }LS Gi fɷmvHWٵV: Տ!  !"*,Hgm#\Dgm8r uzHbi0M '0Q1Tp:@@بcI %SGh4y]TM~Fwlgmxʧo ykyL]94ytBdS?WbT\  lS*!U לKЮ#w=%G!_`"Fp gYD uz˾h#J_YW$:cI"ҽkc.w ׁ  $Dw$=:@@؜*w]6>"$4Z)/TG%4mĉҁ6.GeeDrV4~Q"r "A\  [ 苞i$@@شD_q?n#qgO/Oy?cWA~UN%H«DZW 6GeE0]Èe!  i"^$\"qOi$HBy~D $ѫf Ȯ\1{NbWãkDJ$"O$9q&$#:L7+J$)gCP:eӴb"r@ryL&. @@- ItlKn\b+^xF PB70J裂 'u݈3 aFf$teJ%"(4:n5iwo% @@ 0e6pǍx)p6QuLD}f˝pbWT>qts@@@`t"o/O\#w.J%O~%jh3{$矩4" _k%r=DZt_Fl[^*9G\  c $R ;(f^ˆ=N%`B1j#}$H&2 tY*%z/#ڐg@@DV"".PJ\c@݀^{ov  $4F:w]\[WIh %T"0ywJl"'\w.m3mlr#(JPW(AH1ӻB@@`,D]ZB_qȑ^J%* "|sD$ѥyMe<ʋ^:e֧^w@@.@$r 9fY$Z E Q$>o/[66J3^9E@FԕNp@@m%.#'^FDi**{9KDFT:TI<ڠaTK]heńe#r-IH,КژM   0^GF|fJ""i*(o#*#rZiECf1bNmx u24`"Ǐ("-#   P%@ǶL# }TD(ohIDQ( 鏨XyfAQ-u٣@kQ%3@@.@$r yܧn-ƶtto|o.Fw*6=ayʍ1 FTIdOJ-=%Hb  $Vn4p](pw/l"򨆈yGh " %4s{P\(`a޽LP"j4TTIa/;~7X:]Ib`"K%\(UfEdelW}f@@؞i$vpfק}ԅSIh0"<#ndwNLGtB3ƹ$V @@@ʄޘJJr*Fq e4,Pa'Csԃ4(ߗiHS ?hnt{NAl  CI* m2rJ0$/#\##"aDڊ &t5rfr QwI   0LTe?"ᲮF*pN/}?KcFFVCMFkR5 "O3Oʌj9d@@5$Vnㆪ$T"wo>)GEq~*}7<%*3VQ׊SXH72 /l覑VLX;x;   "]yg>sw[F z&^܃#9E,ܩÆ)ȃ _xqi*t4t˝VUg@@Xi$cFzcOOѤѻ2WC\K1&2<"]T?RBQO# -TFԵHr8|?5t,nZ'((WFU3Vf@@؆U+pglKJTƕiՔG{"FMVF]8+&* {uQ1Qʪ!blKPW.6F   TI_Urg<{t_* l}4pq߱[Q۪XjTI V">j& XX [1&ʿl  k XU$~$z輧?TKلR -6#j[6@@@ I&q[j}7ʑG);KDe$B4#qĴˆMYQL4٨A.cY]6Vg@@X[vUǍLG辑 'T"+5yĹOxx1 `٥]Ф$"rVIi}@@XU+=DJ}C#q#"H%IdՈӊ$FwP>G!$^O-F8^1媇 "M(mf^ǒ0"WV$+:nZ`ByG&VfބQ$V=g@@XOb0"6S*)>fI8(.ФOP6ԟ1JO}t ERZID] Mdג>#]yĨ %WVLD$_ kWi#/p׈V"-ȞA$/9   F$Vf˧1_B ~K;?{_F忄G0BSJ(Xڀ,CzKp&W(%c^zIG4bl$KeOGoJ  JmJ(}=O#<#\0H%#IlJʈ.wC6*PQ iDV+ᕳ G %BP/!A   .@D_RM\bKM&JxaZ02{flOe*Z-ڦ\1Jxif$“$<   Ks]C7B Z=-% iG*ʈ.w@6@YDlVLǕ(GJhXq.#)7(;D@@5 ItMGL#  #TBH3{QGupIѥ[lHo|oVNC&gp,=5|c4P?Á   , r|Jh^oDJķP}Mm##S#ңdDJ7#e#;JG ެ@@6#@$wC/J%<_ '(Y*Y9F*6# Oޡ,ʮ9Sj瀦?Z    yn 7?w1v`m=WಏxZ>cxϬ2"rDčozrB~tH 2}\>«yt WOD?tJ0edk@@ؚDwz~D"$m8|SRjd< #\-+k]0+ E l!R I|z   $G$;%R](H֯>(#)H# -/'i}7/l-=~DM4$RyDE# +%TI ј@@)@$]рn(м4p$i*h#Y'w̢aDhKnPgF8W0<«9ap?(V8v8BI<񂻵 0A@@ I,=foqVG8PfGxJ%׶roXNLngv   ep{i62HH#6n!7yO/4`Jx^S* W(WI&Z4)   $zAD/9qwcaDIC^g"  z=+(`\!*an@*+S|E*FiqDVF~9{A#̨@b-@@@  {K8H}m_oͻkI!$*GHeiĢ,劗-:hdID%`j}@@ h{, #bGJVBo]C E.Кi$<8-@E"^Jh, Vc^*p ! 4(V &[&  }}Y*:c'^ı#H#T(J$GhEz'kȴ9*S mUaG觊$Pb5Gl9lhl  . I4aD͞|ΟjF%zZ#4MOă6y^$]6GE$Єj}tծzUk%`"v" ܬ3  3`,;|d5 #>tIh&3{ #*GtO6;vgaIx\ 3៦ĹO?fTxF:~h<4>r9rdnr    0Gk #m=?Zgzw*;'| HzwZF%kvrΞzDLxx>o/hǍ/"ʏHBno &㛝   0uݍ$FΈW0Tr&|DJD}bvqDV"6# #_}->EUc`4|$}G XIGXM,BQbGe5>"k&KYO-t H+L3;S\TMdDtʮVxӗa#ƃ^iDrYY(5? @@ -ag( %νmY34WF´DBI %0"^կ.JcFq#qQ&4wFdل+&TBE0r M#RR4H c^u7x#*%/$hoŚ  LT`>ĘÈ/>~k*fR[~If4i\"MTQY"lbDx'ĤW*k=8btiDi JxPQŮkG !Ā @@ !J9Z솈`BˣP) %# jM\"Χ}bgO쇚|&<qgbt&"̍_:I?:jJ/_UQH"` Di #"X}E0U  9rd'FhU{nu);p!-$4zghN1'g#ܡщC+gZ J"2߷Nkf}:\A TL7d  D`zUڻN%u#zOё *O%\%HB3Whޱ*įn*x絋>S.MDńS ;V]1VFCE(p xތn\µ(>;DJY#mMS1Im  S$FDƷZx{?-,Џ!-<"NڈxӅOzS^Mѳ^XMWSMW0DSLhyL(p0&"pxB3i0!(<JTD%NbD"$h?Mg)4n&d_  Y`.ѬZJhJ%DBQ.HxCH oY'iZz^L{s0+&G=&4c^++&!MKϼ ɩ 7 Ć9  IT k{='i ݈0<6lEO &\1+`B/xu.L.tw(CQ" %~J4"yP #Y&6o@@XI`,nTo/ԌX9 &T>ݐH(p}CJ &0LDoل~LD:e9pDID }dZ *W+2 &   ۏ$vZ6TB30Dك6l8" %|zk%_o{CF*|\Z(-B/jNb :8M&&d0  6IFtc}>3ML=GzOmDk#/qElq KO(#pDYmf3kP.@@`$#izy4P0B=84AZ꽹Dgmh>}gD-*ha]ߍ(p(Џ-S5BbI $U^وƒP kIzJ#׎  &aĚdn'Ud%2HBmFXz^ЖYMQ!⥏#mwY`b[kG@\K$A1x3??5{Krl^QڈBHBZ]5kx܆-˗VJ|$M<s$hhr7TQ=5ڨNKP  @jaĨ_h= ?O~}%>ne #Nt]"yw#צ\  $&F)SWЌ)Q-fmYyHlDGD* -$b7՜N0bNɵ  \#GZTrt.5}l`KTj赡H7^1奧eéb,>Iċfi°E\?K=m  t6IX`bwIxcI#r"4 $Yd0"|#E! U`sZrM;oPt>4Dg:6JA$1T[@@t$A019 kG+* #$}pu  kN$A0Fe 0lA$͆  @gmF @:" &  X`"[`}aĺێ# X`,Čo2. - ; brh@@ + #0̈́Diq@@`rc$&&wqJ`aĨA@x# iI-c|A$1v@@&*0H`b7FIl9  ILZV`aİ@@vJ`JNݚ\,KFI,m&V@@@N`zw3' nD@@T# MΆLZ`laĤo'N@@`ӎ$&{pt6)00HbB@j.=N.5/{Y!@g1(sȑȆ   8@0 W#6o@@ؘʑچ!\;  t$&v2ؼa9"  [ILl8(s kr]  T ILpo!@Oˆl  $&3[ zp  lK`H`b[ qa䚌F@@`XD6{C`f3kP.@@nk$&5 [!0cˆ7.  k$&VmG`lV. @@>$&4"0iˆI7'  6IL!3# apJ  G` x3A`Mke  I`kĜn# f@@@[$&Z!0~ˆg  FILdXU0bU1G@@ ( D`rk2N@@.  @  =FILlW6G`}e  #  F8C@@&$0H`bB:Wˆ,ׅ  &ILl.л,@˭ϵ#  VEk9a  U`Z va  l@`‘k\/  [|$A0ŻCI0bNɵ   0 D8q F]8+@@f/0H`b+8aİ @@Va$A0ʻ)@U#  0*F8F-8@@v\`Ď\~*@  ؉H`bT'yˆ͛sD@@X*Cһ'@16@@Ewf@@F#/9>}@@6&ӑ3ˆ8s@@DHbx͝vݷa|']k/{=yF̣ @@vJH&ةK1v*@@A"F'߄)^2a[sF@@ &=ahC@@DK&Lˆ s@@،D+gVLNˆuo@@؂ +`pY'@@ʍA02t *v   0"D0 Fsb-@@D#U   #D44# @@@`$kn(woG\1  G$1M@010wG1@@@VHb-7ZXSˆy'W  + IL~V;&aN57  uDk7&N<FL8S@@Xډ} A0cm @@&@$Qzr`h@@F'@$&!6I u   ZSLl~&X1G@@@ Il &C0bhQ   ELaD??F@@ QL1V9ˆUX@@XIV ]ԟaĄSE@@ IE4wuwĞx< a9  [HbK0b)!  $&j#i$ˆ4   L;Ll#ϡ@@@`DkY 7aĆ9  $ۚ`bFd  ,@$1'XSF "  0b@Lv  4 I!ِ=@@XUHbUQO0ѡy#:    _H@0ѲI#ZB   "ub @1{@@@ yLd L1;C@@ILz)#zAl   /@$1hahIN @@vYHbZ ˆ`@@tj3}0A1@@@$7gLFLT@@@`$v>`0@@Ąk:`0b7{G@@ @$)rraĔo7@@vZ`‘<_G&'LBOC\_Ne   0iG=}w昃 }'   yGmk:"ywҗ?ҕY@@qG>[ت$覱;#  S%1Q1QTFFd@@@`BsDb7*&P%AeĆo<  3|$A;rv# ˆmu@@$0Haln!.@@&-@$1w &Esc@@\Zrױj0j$A1G@@ ḬQ{I탉at@@-0HQwյ &D{q   0GM$98RLz?6g@@@ IlCtLFtf@@@`DP瘭QY@@(hh&#@@@) 0[mك_uuf˝x@@"/Tr-[rx@@@h-0Hu"   Z`zđ# И50'   U,f K6fsI7Ө;P*uvb[7@@@XIx3}NËp˃G,KT6vjGT--\?D@@@@v "UdD@@@`@H5{ٞ?h#"X@@@&+6 lzy0Tb\39q@@@ K #=Q-wX:o1.@@@*$`D@@@ I7ރ+JF5Ǻ."   f-H{Jc@@@ #nJ @@@&.vxK8-SnoN@@@m#GXÖ$p@@@y qcU!   0r"7   <:FScF'OU׾C̥"  WXi$q#=c$Fw @@@t$t~Bd$79    zEQ0$"K(O=bgk?o4.@@@c$T"B(g *%8~y@@@N`H©޳aMM I4G wϱ'@@@$Ƞ_G;x01d$k;av   $!*#*&観 cl' @@@`&Gr((?1t,D88b, y   Y`]S\")F!ZN(Ep   ;!0fqT>w:C4xF{}D@@@`MDu9I= qӰ@@@ l3w&Do   pۏ$:E. @@@IL$#M   C +hL,$#Q    1XL4E @@@7h&# ˆ"@@@u= &H0bD@@@`ӈ$`h$AΛ}#   >)EG q//@@@" "s    < @@@@`D7    p7   lAHb @@@$@@@@` D[@    @$=   [ :D@@@" @@@؂9$   Ip    $!@@@@H{@@@ @$t   D    "-sH@@@ @@@@-IlC"      lAHb @@@$@@@@` z{4IENDB`PK !`ppt/media/image2.pngPNG  IHDR&>sRGB pHYs  ~ԌIDATx^15r C8A:jԝYjO,;+j9d@13Q( dZ~Sȵ 735/y @ +~ɷ]h;S"%Hձag?_?I?ߵhPOD<]4 @  u=Elh}2IR #ܴy<ώ{$TQ~ @ L3„1,zaDK#DzĜ$!ꎔ nP%3@ @vQz=XZֳ>*[.^(B%։=̓JB4&@ @J`~j>\K$Ҁ$aM4 @ &pWDޯǪQ %aơGs[6#cI# o~I? @ %}u\GK,ߢJOX= $=BE o8 @>Gf|yD+6qe*d4 &/$ZZ]Xp$)8+lԞ߿r+ie dN L^ Z&bjmMr7Sݙ2C.,C\Mry@eY۷xX]lJ@~ Zٟiꝝa04n*a& }Ԍҡ]بhJ+z*15W0 /!p?6M%c}ǭ?v xqd3l.j!8O5_{:Ep pXb O[^=:f`G0*=]M S+.nHWmuDDBl*1,xԖ-c񎥔$֥ΘJNhAN@i^z05"!o:i/j|@|,r` c&>&c6h>-o_%RDj~:ӧJ)N#A țz~ Q*5[]L7Q˿ӅF~I^,o*Y8&|QK>B _~+aQW"$y7O) ikF SeCӴT $w^z,FDH~=ʈOq<3&K+RQkʣMIB7|pWCW0vXb5WE`/l;{7mJs2wBrNzwգ_}#2;0 \mNԭ[6&\g8uMYT'ߵә>yS"<1b^郝Aꋱؙ!''|U(VA+rW֊UDK&ryE:j"yADBΐ-05m!I ~%B~?IT =iUg7_iW%w7\r!C"  D~ R\ ; }ʂ#P4Y)ÑqIvgZlZ߿eD Υ,gY2O H'pȿ: !XL(4N >+;|L5h]˲6d"+~E B iUu1E^1 $-Q-6qS/e`|ŖwNgo1ަ*1D @(c_:̛Ky9犟ѱ4ˈ_SW: }:Z4Ɲ!/dC˼&an}@5X0$L_,6Za92`˔d\vmh'nșED _Qo1Lj9@[봧u֗OYnDriՔ9,Y /fMN""]G=@שXOa;2E\+z~vGb Q%DGM=9bG]1 UCȷK$cwD'(_Y16s!{m\[ *ac @ ,_9N{rqQx-7q>Q+zҀ짰PUte u8U"',ۤOP%}#ŕ-re.Uo7:pO3 x{ngTu^ Ts 3b / 'keб o싄L(=,]A0DfF+h-gUBZ%b\'8:FiKJ$j%,@x,l׵W7]BU,̕@0.n|˰{Q,(_%2]sםPeZn n>V hQ5pqՇzpZ 4ZWM Z }kREBձ2ƶ!efK2 Ԑ$Np0 /' Um:˳(oЃތV$On;Y\i$ӟEi0 jϬ+{ߵlVY>CkE v8R sd=.Cc]=>NrFàM=B׌YKRNc=Rȝ-P'B料bˆm$:]/]MW[KQƑ2S+ƛXC [?^I'P=)GgX%/O NSSs9 '-Qoƍ3z_օ  @G,.ܦO; ׆㎽ڋ&NDgWmԇSm8p\4 @EG>.,6ܯݸ7& m%JC=eT 7mB @'#Bf_-݉Yp;_o1u1ٲ)eC Lճ@vXB @/"sh S#r{;ղp%z/Y!@ *1 {@ @>Boָ7wDQ.qU(#uoYرFژA @/" VWd{,*%o|S6rȿ:xz[@02  @x8'؛GKLߗγ6ٲ!㟸GHW8 @Kk_u_otUM羈v{*7qm)M ٬a;frl>;8&pLs(,Q =Bz{@`h<4.lZ_%0LǑ)Ġyxrs\E_$ɠׅRꬭ+T*n[,ǮA3[Y~N zYzwO_3 ßM"CJH$S?OdF:R'{!/"J 4g*rE @@fSľRPm6 n[f`}:wg p/z5b 7E{aEYµܳ=}S35]?r[v %Ocܩ>Hzy}Dg^/vPS\VƎ:J(󢒢zAUzq(˜_I}Ħ7,K&>$OtVb@@I4[b:I_B |&4X&u{WwHom84u2:ug:C`y iEdE4WqW3?uDVY^ޱwS,e~4C4OUH2WWcHK9<"?Whjl_oXOx+_D_8V<&ƗoBC: U\w{,RF,5aVo1=:[9,somI)B`;W`G䟴wtvaBohʈ@sPmZS.ҫ z&BHS$~-GQ|Qo7I^="L=h@w|2F3ڋ).]f "ZE#B: +aWf? ]3OHYlU·(Ӓę + E;8@~ZCzWd07g]%ku#_S:E@zIg͌>,͇S.V!S7/o toc8:zD-VկoC0h\Ji ֛:ϻVcQ}b* tB7h?c>;8 V6S:a9 oG u ߯7D[@3 \--ENZRڡeW܇op.ԙv-f$-Ϫ{ix5 W]M6(n14v6Cst4Bgb\Mİ eIgěIsQtn $$b>U:pW bگbJ_/u8PEo)}"&-~UdzAQo͊<;ik"WpXԍ:f5ZvD($ ݬ!:Elx翞jH4_뷉tR;/e$zk}sug;P%fyb@^@ & ˇ~vUOֻ|sT2FVZ!* 1S"?H'@|1Nw3,M @@`@CzD aYfFa>'֒mǿ]w8k=Yϫp1%IhqJG($aF3nj{Fp  @MR}]c%OƬ-@A D+6㢪DGw*\AΉDB#Rβeq$afd32M @ @=$:=0i,9 %%!!M,6dF!IG{U[ǻxۢJAa@  +uAa]J]G _zE@ $aE%!Yq*1c@ @`+kf)SSK7fr:Du>B~?($ hΫ-^ &ߢtUt_/0,!@ jFǗt'mJikVzD`>UCX$*>x @:zDSw۬}hjgzV-C>Os3J$Ĕ0e<61*ad @ `!P{nVnL@-C0kՊO_TKΨ*I<`X=B*1{b@ 4 tγ\Y޽Iy>=>+H"FLECTA[@ @@ aYL5ߖ$s"q^1!Wm}z$1˰o| -N:Tw$Me @ SdK\R1qb>\7.fͧl3#$bl}WC+="tԤZqȁƱ  @ #rc(Ёd{ܛAodKNJ'~gJZn:}9ԩX![¦Vbxb@  KdqP n|O1jj^q?W5V*H?>"}/[.\9OX͂,E\?5\OA{w,DyD \% =BJ$Z$eƢ"`UFHtT (`@ wϏ8.@7O 9T%[6S: زuշ%UJ[c;5m`tT p`@ :򮕹17 ;}JMizď;)*Le7ߊV IbeU؟ΖJuf @@UtzϬ6OAsNgET@cW7"y*5@ [Xc%l]qܞþ|z yFR",㈍@)I;;5?e @ Hsv|wh`]&6ĺuLz?Dbl7{anhɱw Oj%,  @C@y2%Z*_O!QH>-I_^V9lاGwj(} #D @G/}},< p1+=²kۿ[62(ͫM싄JՐpy=B"J @ "YH#ܬ5LPn$$S|iwjlJ0c8@ @F+zD*, d4G,yY. EYmye,7vƝcG}*R+ᘑ4 @zD*٫|%v=ܺ Q:6>زqGsY˗ sXDu6 $k{Gj^قF}aZ @QzcU:>TԿH{eei dk*z2NrVd(8 @ `!G,uhC4  -Sg[*U]nNdp?pBF7H:()+i @ ;EUb_{ _Pv#HzDeiGH!kUEcj\fX=$aMHցrJ`86dkT!L @ ,V".n$,xP%,zD ?b>Zbv'{c]F\VE7+s{P Smُ*a},!@ H@Ĉz[Z~Ύ;0ܳEh:T=Bl>bHg P*d!*)D\[E%0U%C@ @ "}K!C=$GZ,d&I¾,۩!CbkLUb@ DuW@7v ~ص.A!?Nb6ȝ49l2$~$NzDQ븨Qj8T@ @J :p)i`xzu;[8;سY* &"k"NmO:u:U&Hey޽S1<9bHY% @5cvō$%*M2C*HI o?ryqO]\٩qFptjuӌ^qf[n-Fy GeG zp6!IXuQ;5vW'GhMR"9@  `߬!E"~ K0uOXlzO;q %!e~F7P|F'E $wӗ)ΕQ,!@Q]E+gcYV.u=u#,6DUȵ {AwS;5?KkF*􎔴ya @Bɲ>/xp}b{[=ae/#tˆH̒d-1엁=pR`Y:óR uz7\ % @fYwoо&6Iaci ]6.,$C>nBٲac6C@{NwzX!J 4_8Z\' @ wjy|w =&{+| g^IbN Ỵ jʱ-E02  @S lݬa_6t,]\4-F<S;:t)Ic6I)I-1e @>@᳸>ώ:==Vjե7ܵQo`c4iKSE,ׯ. 3*ƈr$;X,!@>[0f#rMLe2ۻ+!JE݋!1{fqH$=زaÖ$ovl"(臎J,OX}J@ py]M<Mt0Ok#7GCmdbưGE7c?w(g;nG @0˩;k_Yvsqiwt\CnȓJGtvmH}GH'햿HEl!WO7v6Afs6M @B`GqD ||wv`u23&2jmJo2o~ȤޯeAdo?%ܺzzdP.CG+@ 7kX*4FM ^ 6mL~ԅD="jLIV}}ZZlW @$)p0  BJ@ҸKM)MH 'Z1Qș #6>=bmEwGC[zWľP+ZA <@FCCzyф8lƍZX:e {᤹#Of>"H}ĎAH+W>hfdM_͘f @xw-*vs[|w.uH/bSZ"1F\8Gb@@ B l%I %&/VQ{byW)d0ܲl:@(I׶;zCJQ;~c{U #d| 7q5RslcKuxTqĬH=ʏcPF@ }|5<vcC;9,B(# oU#TKḱy$XGG;PS*<$#7 @XܬQv~<_üy/N&#$B]GOI18E1΃p<3@ B6kS _8 8h@ &YEr|Dr(~9W;8B N%|P{D6qD2~ @:٥}T1VUb>TJ$K}8ž0?MSC1 @,\[{U-Aō7+z?-l /% :yGqDBlO v<cG  @@5m,"-[Fɯ{4 #OJec^~/:9+Ԭ1UaO @`H8Rbhٗ0-Xl:E'WI")TKV7"OqRn*X*cK젊O@ m:'YMTF\a)(~vGo'w%o+yNWIX#VK+:t 5qR- @ZW$V`F]L bh*ݧrߚ‘CGHP15?_mauO'7$d)F+WmYxfe( @j9_=gg掏)QƁrtoYUGK$R}D#K~zcμU.GKs{ӛ. @*RaX3K9HS=V+m'/UoEDK#$.pW[qo YHC\AG k8HI[hNJ#А%ω$8Q$ՉHP @Hq.ri%}sc{tT2HzD*6,J$#ľĠ!IQzoԁ)c_>!$O^1t8 @$p8bJĮcGKqQP $QSr&4?,(4WyZ# @C@nW3w,c% {ױ+DGHvU#Bf8$Q,_wo ΐ1#"Q%-%!Fi$Q/_wH}QKtG ;MK,7!@wE7+r c%hޅc yZzzDT|_$ }a:r";Սw̘ zJĎa' @8CyrĦlӭ1Vl17@Wṕr=(|jUBK$-V?$\R9fsbj% XK3@ jD`rؔyW\pkRV#Җ# N֫7Gc\J`_sl,%c@ p#c5v`W %TXOl݃okQ%Vr+ MP%#|ZN]z |XgUTΤ&640x)0c@ p@8|2*4Q{r h6>2G/J?꟨W;J>D-66= &١ !`)3 _v9%Bi!A79<##6%Q%qW )SƊj+M7Sea9 @pG4oϱWuKS,1ܵGHY|Q"8>uz{ #}͎MU @f#X\ah;zV#RڢGT@p7M_!-w7L~V^Ϳr W @c R|wblE\j+ \ 9BBT<#I\0 )=bGwr !@f wj,_Q].184bO}VSj6_H#$# IX}͚6|LFDql[@ xNQ[)Opa E:zD~FJ#m@WY$?3ұ]:L_(W A!@}im1\b1*Bb%VI@Or!y־mnfb.fЧ㈝Nx @#kX(ت,v ="G"FP>|wQ*1\n 1I LDa @G\rCa)]ZR ή yFqxU ٸ[0.b--6'< ƔT8X @hqāo_9ώlD뻃Us N }g T@,}qi ֣$@;5hIbc j/hɪIo>1l9H&76iG*'SGR"_%s4kjy|h0ÃI3 `{HXc@c›w\35!BrX.;ҳL[ZT 5Fet.! ӫL#q=[ 0h{-} MKAҨo e{PB?6:D^"?Gr|R_$BYUu &lC=U<A  ɝpQsm /Ԉ*zDs!GZG4f* i騕sayKnS-qۜ-^=j4 @|;5ڱh*)KAWrݑ[DBj%IPK)qIPUB>ʾ:|}Z^;;2Pau0 @X٩q/[bso/RȖpT~}DB]G,[% q :gWQb۽-aNŃ%ra~<;>a&l @x M;5^M:er+7;TB*P*A}cndJ9z ʅgag'Dm{fLYc@O pF i#c Y\"locU {}DET y8h_Cl8?1h!IbX(!Z Q#dݲֆn緌fOa!( @!&X̮S: $FzİDBKQIHǔ LWIı i%o$ݙ;h6ဉ( @ |~ /w謢o;[*֛84Oc %D}R&#Ԧdܻ/$8rT7^dwP !@`I~Z>\Ttb؝WKܥG=*^fKA}īg,YU"#D g:Jd*Xdn6 \"- @@~n.([oܪyaR/vR-Ұ}=bX"#O_-<=sh8EwzjJtn%6\7];ܮ\` bm >Xc @@N ǢQ:䀓]%Mwnz!n4\$'n k%zD_P%xOuCf;aND &B]:X$p*9;Pw %zDD)?̟7D](qUQ T%РMQ\/:8y{*&3'" @{XYQK'BǴen1&p}ѩZHbPA83s'cƕ*~Jh7BOw&ʏ8@\G>6  D*btɭᒐfJݴ#hKwՎ*!5G䙭p`(W~fVS&|h@@N஝SPNL5_1}$ϕ{$+DU^ܵ#L8rehh&,+UB\5Fpܸ*[G+ʏqN w P' F,A !Iؽw{$7{/,b_5 _U"UB[ֽ3_ƠGajylc㰏> %oie[|ZVD'B董I>-g-qU#OZEC@ U"??M)QJkJеqfo m43fmXc]V ;+,!@_E B QT^syBlo_JآXɸr~_ݟiBDJH @YFHIȤLĈeW% %)U"?p[ 3̪W'_^w @x 6 ]kݎBu!˳yy&s#??bcJ(!`$PJҬD'E-5YrÉϡ1!N&0aċ @S ň|Gmb}Y[h#{#:q1a{8hxJ%<\N[% Q%^*)Hn8 1 myџaY}c 0;@x; Q-8(pDB_+Qr%G85#p)ItT4We|/g"! L@l#r5.\*#w# ݋v_YEm#,%,E@8o!$T/\*UG?% pXg"Ll[@,3gQۗ'{&]924ي*[6Iw_ G/a~KEF^"Qۄ^VlYܢ>$\HFQ @W 1"۷?@:bY92b zrsSHGbDJ]s%1B+="7W%:Cd:e1\)0x ̈! %8 Llqr˸J1nH\<\2 yIBU|FeR%ѯE-35*8!QBl9V $ JD+JTbpfQT4+vts˺#&2!nN\yF_∐-b$+UB  b"} "hWSS!^8HJ{4GG&f{@@G(-w#r<pԮG\HO'$TH?>CZqkeGg|f-5-s;k=[< L܂ ,euOgE}gt8Ľ%h>3l=RYRBO )I*!֪DJ {<~bEckc`kp0mgΘx$$s@8L'FtZn*8ܝac4w!X#R84K$zYZxb|tJ$Yah ]gIt $Y=̓"l-bseN01usZ4Z3}!9P=bJ(^fI3Ehn" zP"-Qf`|A4=w|tjky7%0 KĻ"BVyYGGr c0VE bưV(,Sl֘tN`$QG_;87q/ƗȨa;.*mЬOc&1 o Gr4v:ikDrQ?ܦd6] GHatBv;DJ!_vUPBUdܻq;y'PTQ~:8nܠ |u1>[/>×c:\Ita+8")K$t 􈏙tD $Ri%]JJ+S'6\7{/n|{ǧg7 @cA,G%cr3T[ @ MYDJ <0vIBT|G|fIh /“z-c9]0*V+J[@x8#mxcWݱޮzd8Xe`ZxAu%ND>B#^ aNHJ虗CUB+ս,\AG\clNi^8O0 Wߧx\cUo֘:EBJ$,_!Í8'I|*1#/^~%pEYnΚ<-9f' 4"_<+R;@W(XvJmdyYN(DJJ8ލA-y}y=,ֱ@.+A[@8OSaW v"ݝrm8W5%J'91{ $D(6q*1 ٬GC×YU"đGKU"M|m\L(N&ǂ y{9?4t0ĺ{z'{P0nhgȤ-O!䶉t8ZB{[B m8bi^Jn&>xv5@cbň{;GKbS*i{;;F,?S$ U%䁠SB^#qU"!m'qe;+cጁfWؼv?E񖋝7󓔆^ qs+=1&myzx7r<1ƜEi51`ϒ$*quD_(zne9z8pgFiz ÆXx V"a(7͚s`Ԥ d)e`LŮGtJ$dFS`}2`(I\JL(Q+6q\-kٵ["$(E6W/ ;Rg~48 gR"ޭ ɸqթMw))"VHγuznHms=#F87x$!rJL Wgǎ3/rx{X60gHБS GEt?\ QoS̎U.0f *Q"> $+U"&*Ql ^{ jqec?L)t@ArTD,\kάBkeJ-7jKleuxR!bDQW% ݬ#}CDJȃBU :;MWM ڔqaӎWlwoE긂 p#}4*ha4ܡWLN6lKG/b^ypyS`6L5|P#dDGu 7qܲ,]8!f b_ކnGCl?\n%9YM:6D27 ;xfVz(%=߬!}H oȓ>OG b/'>IBT OU~İP"1rM:āTCZ=cG>{ Ԍ İ p-J(LlEլ}겣\—T8*bYØOJ~oP=FA x$!*1<ҢJ0onתxuq~}9ڄ{Xi@ EZڴ㭼0g\#Mo{$%8BX9N(6kz5@J2 MUBNH%y&p!&}`6&8j E8Aʰ-_Op?pxeNr8¨GhDOi5 =b%IB[PU"/Hzr cD8l1(V"2}\=|k ķ8x&"7 ѣgv2+ˇArWSqzrxHGa>x$!DQ.!DpeS<m%S%VX_}=ܩXW"[g-vvO?j*\v+z?iѸ*K$ԸF$z5&P IHgDX(!z@>G&c7!yxTu%b*xb[W45{H^锯E+ݯ^8I" ^$VY.aT%dZ0?*acW&ֈ;V²5 %OJJWw]YXM*oJzD1ob%UqE7;;nF,ޱ6Śѷ7&i4j&u<|LIJ;bkc&Yl-,6@uuQ∥8p@vKG_䊉~҉҆:f "/Jί[FTDHN4,R1V͚ ,}(Z,؈<+#3 |$Dz&h^+D$PB@/K+Unlľ3ŏ_fb_”N8M D4a[v _|$YMNQp|`g_GHH}aDB#䨈#^ 0E% Q%\V%\b?4Ja߷\Ԉ;#JIxiEAJԝY-O  XWѣ^*vCTh5~*ar_7vlH)yEԼ`) !`!ED¡GKNR%w]hJ$8dq{~qEDL3rF cfO˧1Ż(yf2BDD\ /!}щ-oCa;57%nE*,Aqč?wIJ*+UB Umc7m5k;b/:,,1!BHNmʲ:9i;a >o)7LFr*zGG,N(WlhyiX%^Z":}vwU?Tޝ#OL %@Be)a1nDjUfo금GcqeP(ȋ#7ݯAqXc_*IԪ~~;8,X;|X.1dfVD?A_V[FMy3@k D9%)0CݦpD'DZZWDGhqDGhU-JJGGU%,UƇqoE>3}Z.- rj]xbXx R}2|v/ʶS4ۋXo5!/bTUݬ#/@$rs\B>N9|^޷f;iRwjU[tVnF9 |$ {^OxFQ4Ecҟkwqv6ON |U"!uH5lֈ5ANIUBơ*Oǃ9K %g}a=>-rK֓힜o,z Px(B{}L|~ƻ7t;rS0 dXZG>zDG5l7 >$_&>tkľ9Z+I_qľ5=k^ZM8b*)گH^qj.OP@>g' *+}^ooַ??pyHj8=uq_6k$~PO+J\0Jۨ]p`Y׀ [ |iߍMu)\C!4Ncs%0 ]\i[w:>b8#DB,P"}#(MIAjGR%:vU"*q.8^QjN8>2!Nna5)e  NĺDnI@l,5oT:R;a_͕B21G<ۤG,)XSB Dds-/ç}%X }mhЁ9honi m4ud51׹lȹA+.#דq0O~sN>_^ÿI ($5$i$.֛8˫r KH!)y>I:e넍nq>.zqONiiH.K= =GA\%3\ET 텨;h?Gň\TuT8B%zr"}Y80IHEY\JzDLUoR}R.!wҕVۿ)]qV|ni"AMr ahpQh&M?+F,vP4##4:f }g=5 !lQ.QB I"\bj z1nvKs2| gNBAw]TEશH(l(Z\E%c5ۭ΍T45 )(z&! Ǭ1;.C{w8b] ,Vdi'%N@%rU"/% %J$rUB y_fֵ.&ccXty=ƫӅۋ#xO(Q\rɛ,b}a95/SoɳOTZH~ !~%ngt (48BXJ$ɓq}%_XB<$ yDJ*ٲ!2J?C!oc:QLwunw+Wڮ>є=PFa [ތ6-8@+9bt8_fvS?6I{aNo'uE3U1#8"݊k=  ILr Q%101|Gy YGZZ# Z(:<_e W".@^MQAޢBDE}UBDQ?U/DTAqg=HrJ\K ɠ(д}o`'offw+Wxڤ*PA^SLQx@ssu*ʁS]>WWظS,I.%=d*)CuHNwvһ,GnWuND^QGI9%n$$_KtGzCU")$O|M a&ٞ~NȒM,Ax 5˼(y KJHgv!$*!+Ub MWzZ5(86@$%yD#9%ml,Pe=ݟ$C)Q|\Y")}cڎK:Zu J*x?Pz(f҈ZWhΨZH'7r0胚*P\6QG٭Z;5C#7@/t]ُ^sqCxw ;M@P+ꕤj ZBm֮Uk++睗ʩW[wK4 X;5{ -oAqĽcMt@Xk[ݮgR%RHgNFXbYldO)r;R,yrUGi҃QC,Gq}Q0!ˀn(iD.Lfk=zmly rGpr^!(H!ԵCΕǡ ;?Iޱ,3Sge4j*FXv#b6$Q+G`8N(yxWJ_:?Xq=2 .&c? ]&ҟD­W:@=b9Q(m9< dl pH$<4?K"/ W>$sGbkh0 !Q~69zh)m>$%U{=hz+ {r. ̨gFbW]# fȜ'F(?Q~!ԙo 룩bQ}o^"!Aq@2r(UB^Njqߖw+u7ߧFwe;ݻ菉67RO?%2~*))h|J+:mmCc,6ahфfktJW?EU")D^:%OJeĔzD8B# !@HE%\")D_0JU%N޹Cr,ֱ@f;Xr[poo|`K]0jfMm֡Q1!?]"] |6猾#)VJDKn'X8h_73>X#(p!HIb&<rnM8!!} q2 dSz7=Dn9uC q&[_UGHXbAS(im5WiK39}t^B"8Y>\h>U.hBSpoF3%Q%K1814[ Ilt &İ\^(^XwC.o"3 t5Z=>~>7:WM,c#nދE㰫ғGQ%6p˭CV27-_;y+|p$oyK׫݃>g2^%5XΎ>@-TCU-F_ʓةCM+aq*Pa‘H *[phҹaVTEj4qĞ8_nᰄ@ e)0x4\K{땾e3S9&!\jS-NuvU"PTN$ڇ)ZQR1=b )x5$/ǡᐜc/g %۱#`}CpuxcS}fb2loy;Inu~ClŻչC؝ϱHbcs:@"FH+^ϡ0Վ>E\"6W.C{4wj韈+i 7@xĨ>} (TN4 %~'wgᶎ~dk#̍}wX!S'+C6l;4hnivlw!fǺ>a\aJ.\Xΰm%j1b #$1%tC=J,Mx Ѽ\}U|_)2ĈLuB$>? Q% ajP"/І* n|%GeռY[*p8i? ]1+!N?nxGOd ~ol!nrEĦ(XJTL4ňPoĉ<3}>S䶓gjX @ $9S8`Bw\²}T'k:Pȍ7Q]Yw|)}fJ|Ci?b09Ҕq?^hbs aι* ; =6\@ I|ڬ(M&DDCʽCK^"y 7'O pNzw"|$V>'n~ߡpc0k$ek5۝G !p4w;5li($ >\&(ԅ-LHs]T糲 6sM8X|RX9I 4oJ9?Wzr͆ϭI*CWG=q/'$ &%Z1~. %,%WC( %:p^w7\]qi.ۇ؍o${1# ))<:u8U@v|Bhu^Qw~9|%93sʒsi- @Dq zrR"/-zS\;]ywJq}lbEpy+m* bEK* |2j~zY<_M4ňK-ʽeTD@@9L8Rr Q%_}U",8y>B+|G-7ͶY/ע5\mh;"m|UI^^w;XµYwDSC-@1  I|ȟǑINQ&,!i:){Hg`Bp;ycg=)3cnUeo"'K$65>ccwf%\XEp#c#liX IX(}ujV^.1,PvWģ$^ziڟs]'y2eIO36'*~z[%|Lܮ4c]XCJbD^!+3G>yюGk&Dr[ވ[z>SN(Q"&-vnhrkF*gDEW'B'5{ȾQ=/Ʋ7uOv,OF⍠D&*jH߱.*FJCŎ7@`,Wa"識ID_JhHZOh[ܴ)6E `q[[,m޸0:&-c@\#g$> _i{zCYZq;!($ &C0QI UODs'#Og.\Cp;fGy#Ͻ5= ][:V|7$c^iW4:BOrWvj\09:~W=Y,XeOGz,}FDSӏ1tkvkl(fZbhLʕŦooeLQSM:Je8FvߗaDC#M [dz#cy巤K`UXvK+JzV4o&6k#1EoMFVͤ4sӧ/)E(&y/hAd]FM9Fr*zv ߅G0OɤoʐI!XK[@`$}l?s-L*!%kxƄuJ$Ġ[΍du_]0ޘ͌dY [lV263?@W>JPsmyKI,X'3YcۭQ1BŠiٕ; )C' IX0g^j aQ(Q%$ Y+.8 !|/o^gӍW0lΦL% GN4+,b Vdґ'LBg#,q@ja"/HυQ'ܑN3N&j40)WSox*o3. [qܟdLj;^?ߒy2D6vB+)Os{" $ 0QKD9א6l[*&vܧ G _Mܴy O-;ّf <%L(_@ų!fݕJm+| &$ &@!LQ*&TGn&Fq;#@)0vS.L:[wd[ōSCs2TbJcۨL~ GDSȕ#86⽳!$s#{DrZl0P)0~Y %U-L|Ҵ&^7ao4I!eؽZ6VDMrƚGiֳ/Y1b6d_!@p  p;$ۇCȋr &*a&TBs8@6jcOzmmy~ݩ ߛy \7rq)bN|:Ӌ(fE90b8 BB1b@$<:M\H?KD(L̪,RJāw ,WwMnI8p's0G<z[_W+h*+@FH7"L47q0!=7VLL=M*5L߱-cvd[Z&;/DN~'\kOSrؑϓJ=oYfGOپۣo}9<1ۯfVF G빕LIɣT"LLN?[X$m#wjPMǦzy $9ء>vK v,bӣ4RE2V"#;CH p@p@b" 6y+D Ѧn6:r}mau&0/I!@+U}5Y Xl40eL0 hbDS@M ZA'SZ1>?K2 ?SnN-*J*:M6CePNX) $;ޮaݛёٹ#!NgYAڝ-gka4mB1e߭WcZ^HpʾN!,þ A3k"c(#n!x $ʇ&8̡0$ۦ*~"LD oywr?ZK[p4;$Ҟ2Njown}Mؒϧ'@ѓ% ((μ_o$a`eΦ!JDBquC[9t ǡ[9sto4K$+Qs戺@!Jİ#cp`ȞO4aMni& Y-ibrcvYGWoўJzG x $ԧy%L{pn,οt*Q%ҟDlMxg/Yz6ݤ"i'3 f)64>*jT9gag^\X@'$1 ka"/6 *)P|WjRa}{0"QKJfGsa{0R:^sE/0vgElF%"}ԆR(h@CIci Z.&k%D}% FYw)8J2ɇl-uͼ&>MKc͡AgZgM3ͨ}#+WF Fg@$?Fߒa!L D^1DB!k%9a9{S^?Ax fޗ}94-b͏X=Wn"DGiPw@AI3sz 1req ێoU\zcMϱ!  twvKy%Uhf!$E rMQCSIoSfMnѐ !Gi Fǔg@q^00W1-,TPH? }*zmePG7%2^Nz  4X㢏}Ex,sP)F-Q83"vV x/$Wd u Hڄe2JM*u'mOcЗ JWچ$s"Kٶnzr[^^52ei 1"/P%N߈դ&$q8<7brdc7w@UO妾In v7Nǚ=J3RbLŖKHx%D5-6D!/JA^՗bYD=BQEA6G@aN0j.LUL|& dUf۷|2I1C^Csor{r*oB<A!7'A "8OH x$ DmL'9?e׆|x?RQh0y/wKn-O#nn;\c~]9,bH #!TH#1 $ $0&\L&:4\?Mo>NI\}$ARAsGcEMӯ:筽P𯚧@O1("!$ň B!F0  =~bK ~87rDR%EHwW}F^QUD4}J3 tu̳7Z N57?Sc!=]_էaacibYlrovFKY1yܡlx4s^(ԌT/M'畫WyhAs-NyäD?1 $'O !8Z+="k%hHĦ nEP1"7y"F@CI{z͑I( QɕEPabCw$$0^q8=@hxϝqhvjSO7껄[ Zop0,"23@FHFP@CNrZH9vv/Ǜ凜=0{S:,@|wЗg{mlxi׌ Ͳ.F|$yHn\"$R4)'fYvm1JMtCGo|Ӊr&.B~ׇ@WC1bp !,4܅\ԧ[@@uP U%fP%R}ޙ5[ WWGK*FmA3z6=p,[lصd)xK_>;Oj]8Aй}MrgӺPb1ܻQE{6rΔ+i @vHvVX~&cZH vUBJ$4^m\Yŝ-&YS9 o1=r^;Z<*bvDG\ V"f Ic fQ$JiqUOJ4ǎĊ2!^.jZ`!9?vD}X|2V-[ga*>?ȕ DYD,sA 1VMয়~Rx$a"/MtcAfEߧލgߑ ͺ!Q.)n{@XIA2ﷵx^` qJs|h\$3lC^͆ }?#DsGEcSU" V8;>~j84 NI")?hCa"و6q%LtTk _WЛΐݒ1p03gBXI`}ټa%s"kQacbF3Et%LBvSBR(9ۜ|LJP%"œb q#'>'$%BĈT4<77qŒ-zs㦆Jcs&zgӗ: gD<TW KwFKnٖ"Hi@ $3nzfM=P!L6w[T *!N5{l0DG#|VH[ E>M&GLǡJ_ nÇV1"G8G"ڗ;7r;34 Ջ{oU_f}4J!#0 { I˟B(&RohuUB!;NG2uVV;"N57!Faof @z&%bvFF.CE<"%@@`z@ @hBOÈ0~w]SI̪) &kqR!moA wh4 s~YLcfq|JlR"f{}Dc>Q"f!c@i$6" rm"GΘ(:~9ڄ+W%yMX꜋u;5+M/fK]C0zrܢ%<[Z|P>"j "ZmC%ѿO?ݔ%DB03zHD+P33:Qb84.w?g'bhM:?n߱XW3"!NG!$*f2:]l>GCppn wt#4CL]=#'rNDq Ii^>>C;p|>[黯AH=RC^DIEEKMDB&k%NmB ҉OӖXU'OWx8+,3R]yvD'Y;\Q"# ! <F|6Q WĢ*(Hȉe"LtrԈZ!XKpQ??y7G:<Jg?l5rxsQa},!|$Sz DS 5װ|ADy7#_ 0(O+qEx֭p "IG!qt"( g@x渐~!PkWDM*㸨Jϔ'|d^jcVw t>a|쩮 صEмN0T"R= %bX з ܓ.v3J;%1xi_꟢b5g灧rZҳ^{fwoԋ_lCt=oΈp5%!N6?/Nj|RX>Z} '2,  %$CO_O@T4&fqU>k!g+7zlh'oHH2CCeYZlV.븻G$*aAi?O+.l+&I\: >D,o+MBD3@HMH7rb)V_.ё$$aD1My"vu"7M&sn:tp`ʹeINfXtmfvtA>7N6(OMK-ѫ;Г˫HHSz%p,C%,!@ '$|ȵ 5ڊm)qC_Ԧ+/n/ӣЖ !It WYCYP'+ᚙvbaHdv7OW8%C9X^f^ |7P"DLjH |&:r)DRm"R%7ʳŨ?m\|cfX# Ԑ'ӾB>.]/}C\s%=5I`Ǖ[(_$h} BDb@CiV/hvsߓ8,ۑ%Ц)a$C跼) }[p>U-hI"p v*$] 7rv\[Ba-NQr琧9XQu!!|V Q?x:.X@f4qڽ☉?Cjr3׬NyΖ|iJbH2 U(R(=҉}b٬fsKɨJ(_g/JFpN}qHa EHiO PNߜ LXT+]$gy\PD"_.F"*\~$H"I,ayhE\=GFIxPe.=9#CX6 HI"& z61H<&a5jTu~ԤU\Zn$Yck/b²oN'_2V9`8cyD:KQ"EGW҃<+ZsCz) @%=@C:\~Б}U¢8̖KXV;V$ i ܒ#s{E*Y\M}I"$]BtiV5_B UؗT8~}a @@;A!&䝉n@@($P8vP IP(~,-Њ0z"Ʊ! g]#I gqPn\r8&`'1[0^ߨA % d1:]'P\moV s>ZH]KLMf•OS]!IV|#b:r%PsVZ=JJ{X=1$|DS@.l @BI|fŰhB bUp-T\)I@aBVHMEПݒD|*Q\25K6"OHyDqp'eLM3!@ I|@uEb㷮=9 گ4l r6#I #W֢WHKl^(Lײ-H͜JD$>!  bjc @ I0 'P( mBiم"k.&r jA~o&ĺ0\:Z*&fo 1||2z#.ʶ`锖DH,&p4U  @AI&[ t䉫.5 5.D|) D0J*L8:K@aQ$LzA^_8:WZT #J9 cĺ$Q M8b *O 6Ho1}mB(!SDJqp"LQ~q'LC@V&Z`Ȑɐrڄ%԰tb8~tQ&䜽a BI"#N O PW68"{Q)$$)a¸n\&'nz:( P?s]&wy)HA!8C!IP OIcH =eFm"2+ZQ0J-L M!E[&pHy8,]$›heyFu @ D,OALJhG\ Q+l=EcQs=gO&zįKkyk7eXB2/SQ+ׂBHBFJAs! @`:C<@K)E.4MLD4rIBT%r+ !|Sl_Ter($] q%=UChw UC I|IxBї$46Q<(ԈJ „/Ĭ0Xf ˮMD1jFjN= #9A0^˘AOIcD 8D涎>'}H+ZQ >`JX a&l㢽94$(=t*<-/~M$u@o I0# &`)3)~x_Gȅ(L|ګ  I}f[ BVƯ. v0(MzA.L }"I\>\ m$m/ :%թQ<P )J} &B|uPS>u< pbc@$)\Cx%^q1B1\T4;A%Y2- ] T= >$ f o',y tƮʎ|$&+Efv q5`$c= @+~ZiL[@ @N~ϟO# @ @>:GV @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @NHNp4 @ @` =B @ $$G3@ @V IУ- @ @N?9  @ ~1w: ~!@ @&ƍh @ @@I @ @ ID @ @HH@ @ MI"( @ @0@0@ @ hHD@ @& @ D駟 }i  @ 7X$ !o @ @8O lz#" @ @b#; @ p *['( @ @/% IP"ұ'm@ @V%  @ X$#;d@ @$L=v @ <_hHP7!'C@ @8)矟1r @ @ēDn @ <F @ <c @ @LIGA @KICCb @ O&$ɣK @ @%$ء!1@ @'@ѥo @ @xА @ @ I|7@ @c IؼwϬ5k֪]vڵg==3]תP( BE_d|onv in_Go+y,^~xلBP( e-P\yky%!mc C1Aef w= HƂ,h@P( h4`Tf0h&hC'ej J:CWk`ec !58ЀBP( h#[bDhx͕ͅR-4d+ ]!AҬ!-o Jf|h@P( h4a-g& n5d88Y!5t9XU4kȨ  BP@2y YCZH)XCOe`k 11RWeH h@P( h41-nb8k(4何Aa+5>G26D +BP(Ѐ1705 4Rš9 h6%!!BYC2t! BP@q0`ӇIAà0bL Ei )@95D*øАf ߒ VL_]tŵ_LæeK];1htzd#^g?NC`+sr_Q!@7Wݪ~U䅡!<9LL㡇# a lS{ y Qv䙾 8P6uU 42 ⠈!ٕ^~"NI /ٚ5hC(S,.6Mb,?|駨 ⏳M+/XPj2O>Ob0Oy=AAC`:WzNs͐;YCɜEݷMd58/mhiACF ( ]?L!;1858`UV|d 14˘e?.4䊖ttz-@CɉD̜ 4` s MShoxD  NkH>i SCߒemE< hH/8ٵ4,05pZCa!|vIBC/`ȋF~W̫16g>pf/}Xj8??'`MdNWuvDZP]I#x~0!#$8?} 鷾]zU=sK ?B_NYCC[g_>Y|ھ 8O+K/sFj ^IR_ _s! p z0δsD+䆇F@ADNp.R䶗Nnpy\04'۾X< ޘyAC뗺>OM>CMx(cxiO͘b#W#NwQ¥>}1QCX=N$)HXKQ1% :`(> \,| \pv XS|:Uwai v  k8ACGBh jr '4pA|\Oa ahu|hhL3BCE}<: ba'^;$0rv3yOА464S?Pou!2֊!!'bz4DF`^azaE4 6߲Vh𩄲'=(t]Ae,4Hnxhh 5t04tv}s!֝?l.hfg^gW+3yHCCxpPᴿŠG .o}>AtGχiGrh:  @C̭;\[g.:A@4|}%--㷙 zEF/a'È ${R>0.8%>K >qX3 Pg#AA;Vh& K LTf^:mIht- e~INBC[ >wѾ_üN(낑WCP( B CF5T 3 u*2/tn~a8ny"G&+BP(ЀG8Cꁆk]⭡rhz%nGne4D:^6P( 4MRBC5TR36 sb+ >fAah`kxWh@P( h4!n݄jh. nH4eH\|)YX7 BP@ 4T ehS2z*CJZ!` yC)>h@P( h4WR45XCeHrACr':$XC<1!8 Rꅆ-!n%8_Յjw:[,&̌W,SY~ )|tbON0VSJ׿3:EU`FW#B͓jw:[,F^O2ot^E5A‡|~{iHh04t꣧ `U|`ϗ;J0:!_jאXk z:jh(48_epfx'rޜV:ACqiuwE>CCa <R'@wQUYy&N|!yg~>0n໒ p H~ŒX#h]W] | O8OD_ ߗw,CY}XR$4EubpPG> Xib!|Q\t7#fz+1/}?w:g^YoS|'dlCK{_af[b.^ wx(mR>eY|&N"1RhHfߺSL  r%2wiBii91N4(k,:r^'Y}N3NkP~h>/4CBJ7OK*K(~,| s² t-sh%k3ҩ QR}~e4$9_:\}!ax?~x4dc?:isg~_ 8{ `WIH>!E5Q#8EhHx?rZ4t@CaĄ;4_l"s¡_ @C_R&/A7KN _>  {:U-x678!|"bNW[8!> ~1adhH1wUhL3BCE{8Ys1 4tztR \0 W9MW{`OSzY䂆j!ĬBC`:(:uа[C y!suCLGyDC.hpK~~7c AsI'7tM4t&# ۾t_[B '48bm *Щs`@C˾>iWΗڹ!F:ilޑ>y<4W v?Np80:4tW?E} =i%]= 0 |6N%6H~J|:A:ACk`!&]!# ,Ab~Xac+4HeX5022|`AB* ]>B8( :AC8\F hpnf9Ihcz4$J ц\QC9 4y9-fxh_/aG'KAY4t  C/ 58yOxdr !r}Dm[awH[U1!!zM?7|/g&b!UGh˰DC ڠ6T^^^U2~o'ف""7L%]$›V NS,H!%Zi3!( }23 E Gþ.d-Ƨ[ ( كIovhHi F<4 "=)yE^hh]g 2tL )Zh`_ j E1*|цР!#גu_U AtQ @þږ}-XҦ= а/GC}ZWt}eGg_f~Ϻ]?{_vHvЩgAXM_ޞ eV?2qiAC׉qd ÞАtUAh}PSO\+Zk55 k= "~׉4DlÈVh|#3lB ӣ!Pyu da1B. hi g`O>Nes@HF1{[:Cu _4]-;:aXa'Fr_ǭ׎1Su}׺f8~]6CtD8|^qNO c tzޖKŴ'gcp%<:5);ˢQY|*ĸ[w%𵐔v1 ں6_]i= #5v|WBcQg!ۥo]CC%a" g_hPvJ [H2lP/ N ~"3X`,4ȵh 8Hbp;D@ 2Xq=5p2B`3 Vgz1Z6MAoŮ "ܡІX!BP546z݈#K4l6i`]+#_s _=a[@B !A\4AbbBTAeu8[31DgHN\P:d15.t1;Yf 5` h@P( hӟUƲC5yPLÇ N7p!"'ϴTGҔArCk!15֐ z*aa\eBPLe:q;aqjfk`nx÷!Axk9DZC@Z!gġ'4d6D 7d8,_ ( B vae(o UsACҬ! MC2d3TIqogAhCr!ꁆh@P( h4;Nk)C5$s]X(|S aex?x7Q_(K pu!yE%h@P( h4Ag(i n–eCCh"!hBBCkHquHq eAZC%i.*Q@ BP(@ :e\`!9` ?ZCqC@C>V`E5DZChȫ ]ЀBP( hSe !G9h 9W\=3 tɟ&֠aht ʷLxk+G ( B <"9 ear`&_==\ =XjS$4Xe:OP8~7<-CFnp^[Cvqc % zЀBP( h11hHQtb!>kEh07!tr-o C=_2P(j*u ?{;'_2(? 3e 1DF!2 E 2red =!3 >npCzr1(Rp!ACO*Q@K AtîgG>%]Yh@04Β@ N!md = >h- 8`f+o Yan𭏈L=p!c!5(Aǧ) ( h4 38|Ʉ%@P X˩{XXZM*}x@2$[Cr!"k!r[> PгCvk$gl!HZЀBPә6߫ ᩯ{#rA̛[FNwЀbf2D6$@C2 ٣*C|A-gb \BP83!vp;=*C 2XqcyɌРȰ|g @ B4N;fz=y ّy.\2Щ5F.heZCxDCkH6 ?` ПZ |>6 |;Sk q 8HYY5Cd/E 1lh@P(Tvޙg̴5gHjISh4gȢ !T aqцdhj qK2<0rE­]4(XqGոk~!| MhBk'pN;MV Cl hcگ|eb {=S"9X>)ڰ¯ NhpC8àXX/ C d28ck_giK3ȁ5ЀBPs &hh h4`L(0 CXi hWZ)}NS8!iE|! 4خ K\F٣AbՓ~9nh@P(@8Ѐ/0G5d#5AZCvn)(Z EaeYa5U!ܻ! _ =2Ao1E TҦ~7 @m h@P(TihH[$!>[gt=_[h2A {[CoWgWhcΐB҉ U1N 8An6 M1Z"'h)Xe})l  Ahg-Ѡ.F}B,7#4Rh*c!>02$[)\ц}7HF5t]4Q805r2& 5"dxahN?I[)>hm~ٲ! k`eeAP;Y:@: h@P( 0@ac~`ihHkHF2 얖e(P7d#^?$4Hk|o hm`hp*j*k;S[!"o"Tl 65C)BP(e( %CxP3ZC+7DZC+4 epB+øP5D"cs+цC5XeV" 4P( 4`ҋ1G'*Yu=E2hH^FAӿ)."7C:]5ZRE c*o  BP@, e6ArCFk igrC隻ZC8o v[om>K?Q #2P( |+K4?  U@ BPuA!r7M@А_^wCo>kCCvnChP`!R ZCvh 507_Uaeh]+| [inЀBP(T0҉>h[e A/ hbakpCk n0qZC$7Xhe -!n jK'::h@P(j~(Ñ\ AA#w`3 F8ȍ-;)Da8kb 6ЕN  Ne4P( U/4$hDK0 4!JZ  8jFs⩿{pq^ AAl(ä! hpZCzkQ%f1 C'huЀBP(4a& u%1G- 5!FAT a8n`h`k 9<f2ȀCWkCC15OH48=ABP(TLcF404(kH!ABYxq8àP$7BC -"1(e4{+ 1b  BP@l-xf` uBsCh l8p!A 3` Rb!m4$Ceq@ BP(@a @C`b֐"!fICT Jf&05n5akp*Cb ZC @ BP(@a֔kxEhȲ 2h PIk5(hI  h@P( h4NaPk l \F|68V Cf觘h eeSCe$1P( B sg·;4Xe hh 5<7֦OGA!j 2 }130Qkp\hА%ȐQCj h@P( h4̸265XCa >hx/8s 1 `[se>}"A`k eE y5t3P( B W0 4bahX,ЉZ!~Eri 3pe 5@C<7/P@А ]  BP@Fțk Nek` Y!"!l 8C'epBԹarsCrPTЀBP( hke(o ; ` Nh4thxh,p2044WrCk( nҢ +ChH3P( B ^4Q4tbАl  |цHhba0kHn(o )ChYP( B PVXO$45t@>6 } 0nNhᆮ"kVh@P( h4Fg 1)NnHBACOe)귆N(zZä!Y ( B PАv=! BCOn4hE1& [C+sP@  BP@ ޓ&BP'4Xk`nH,D>цq4k 7! ц4k( A3P( B Pi@COkHi Аָ!zBYC2 Sj!5!;4T!BP(0&a8ksV?5$CChCU>Аl wc 7d4e4P( 4%054kAanp(*8Th CP^F +C^kgXBP( 5e2!` }!/74__<7 JҠ5T 0EhpCOkK%  BP@ !˵uVePkȒn 4Ze h YZn @P5@Èʐ ( B 3 a 5$@3}AhND 2#&@CFk*b6QZԡ ] рBP( hGe[hЇ4h ںEYä ateo  BP@{3BC.kȿ #2i NnH4npBlgP!4pF 6ೆJ4dQ@ BP(@a~ a`AA\IFŠ) ZC%ܐ`  NnCC=2 ( B 93d6 ]c hD  XC 05 0P@ BP(@PF,` 1sVhXAahhZÌA^F6eHeu BP@Cuʇ>04ZCFhJC5@oEah(c r(P4(nP2$@C8BP(0q>02d!!1p! 59Eyk7Ԭ A*BP(0/PҬa8hXC2tVhmh94(kP a8h(i c:Y C\h@P( h @Hhm,7tf Fᆒ0h5Ek(h@P( hT Š5 k"'7ؘhP>05xkUh{N h@P( h4 PFxeH֐"s d Ne( #ZCIn(` u* i jk.h@P( h P)4Xk9+}JdHkhwi$4>hPq@ BP(@05Qk AMx&ٹ!Y(qJLaPk w<5_j2P( B 0 4tR aYYc`7f02Te CspP 4=nHbk J ( B @(C֐ p _l-~Ò*kba2Z!fBP(0;*p#BC +ø ]š:"kh3 r0"4KŠ!;Hk3P( B PPF;)C#fd6(qh% ,BC0PVk4D-˔ e  BP P@C4S< ߁`!2mp^gAAXP5 pke !F ( B P04I¾AI'nцdnP(:C'n]6؀@Can4t½Fp!2h@P( h4̚2T rA(YpBR 78AFb!Z#de5,P~k3P( B0JER }+AqO RDZ8DZ? @T!#7ʐbЀBP( h@5CCB4xqh@b;MV \"}0:4Ѧ q@ BP(@qZ!/+CG,7@ZIp6!` 1Y2L ra8h'? ( B (C Nnmp _!2 aГ0 4dY2]h(g4P( 4`@BSw_ - ]AqC$4 h3`!T*e5k y\vg֋P( B0f]q^9FПцNo V5t @CU` 4d߹ۏ<8BP(0ʰ_RLsNdH"7!87$@>k-5􇆼Đ UYq@ BP(@a%:305=T!?w@WnpZCk2 7;?wHϱ`5ukH8[COhN  BPs P uTϺ'!FцրC5,i CxnP֐2)C=)h@mׄBP(@8)ÄakrC|!l ChcЋ*>$ҌD$?a8b44Xe(nЀ5PXCs` ( h4T uZ [CNj%䆰8FܐexfhY$"f/?4 M }atk1h@M`΄5P(P2i )i pF"AC|AD)Z:-'ԇnkW2Gƅq@ vfHSpvC>( h4F "Vkp*[ãVoD#RdޢCҠ 1auԄAY3`qG.ڽr;@ h4@2f GRCI >q6؀CʠArC8Tb h+J NkhbA\05g4 T+)F I7v`kt@P@l1 @Vk 1QVnAVe֠ Jꄆ1 P(Cyhpni h@g e|^)XI h4@jc(` B鴆@SAqc'kpѭFba! CFh(l  U7xM4;8M4YYæ&B h"F*N05hC ,`mpøsY@EJk-"BS A8O|=g_x7Ȍ@P(@a 5XCMj eAq|hr7HqY&d a01dT֐g4 ;c `O8 e q< h2@갆JBC<7؀C&xXC 76^NB_Ca0hЀYh[ 5Q |ηSA{_i `^XCGB "0hHk`eЕ[7f 8c j !5g4& z5!R44P(@a i05Ła,hpG2(kHNц֐vs` NzD& iq@ а]Ykh].k!7;mb 5|OP(@aM Yn+5 bAC2Hk NkqC1kyCse  Xô8m6=*y J RwѲ C"BC8Ԡ4P(@anMb n(5T =!F57$[C PʜX+[lllYҔЀʾiF1ne n=?m d ~B h?47nuˊk  !MIoH FPaЀj( 7W~|-l߄7FBYL4 B ڠAe]U>9i h֐g05 }y86h: 泆n8iG3d v _Ȯ?_9C[Ax :45pKHgHq5P(@Ÿ=5C.h(` ٯ?5T >YYC2HnXA!ϰ5a,k4$ _ 6-[}2LyJ p B Oػŋ85Do;vBP@ X@לj}5pla kjgPVkV (71(h`t%i_}6j%[6)zC7Pv14(kUZ=[]  h4@ YhpMj ZDaB0 %h@儆׿h/ ! ٹaN\9۞5 ;{ w^3DCd_r6h4P(@aZ&WC@P`Qԡ!L ꒝!TˆhCd!; "cޔ5Ѐ* 4xqG %4 'hP tlo跆Vh A`r6 * _r_}.Q(e(f e:MN@|A^'Vk nOa6s = +3Phhs靯<_75L:.?@Ɠv4GkP}2\CrN [Oщ'4. 4 2!>Г:E5!l SQN05nh04Q}=4V^r3 ~MP(?o& %!JBC CZAYC!цPRfahÄ8 4%扱[NZ6(kTE ta n7(zÉP;$+Xh tth Нk5v;BV <4$XCh:ql)!9 >%h5T0 4tq@jh`k 5ײAΖ{IJb`e_+?Zg ' h`nPРp a/7%^)vq~e;\:Aޫe_p h&*02$XC kpr02L@C.kKZCOeȻЀjsZk)$7;AeClU45m{ڧ6SpHV^CᬻBߙo1]!7r ccAJA  Ѐ76e.4c e `! ֠ak&g !5L Ԧ7 |-|!7ޜtm9<4[+Y@M5N >hP49N4BF*hX 5l&#a@B'W?Jk`hhN]{Wc Ѐ7!Аl *C 0"1DtZbA XaZ֐[øi S3Pkߦ Uk *, O% Vr d z7'{euNe,45l[\}}CӠAՑ@E@/ (eDathd yEb68[6!F2Z1 n }ateq@jh!͢^_Ia7C.&{54u -7d+4Hk'2i M+n4ks04lڇ¶l##CV?ABk# h4@*PBC5dt!mp/& 6 }h[C%MPsQe 2@@Se&woJ7A7tpl=oݺ2&

4G04s|I'onFhX9}VB-@4gJZCUc =` k/4l BCWkD֐%h@i 2ΠA$7weHhആb/vi ϥ܍BNk{`exl s ε'U@U{%4]' @)AFԙ B 34-hMbL}"-,7ķlpC " uZ)2q@*?4{N ?m]ӴgoMk_2;d;*54쭠C 7Є'TLح(1?>C7 C6@2o@ʠ]!IH4Hk4P@Be:!l iX0 ChVeGŽP2i e!>P4XkbЀ l \ zBZk_CZC ^)? (nyv@$45Ж񤅆Hn`hP  ]N4lʠ:518'4/4(0?ԕ!U0Vk}"֠ЩrhJBC5Ԧ Nh8 AOfA6u%X6gBnyvni hY d >h/UrB"Wx0`L4Xhori[Bn| Q(@a!|Xi $ h/ȠF|k S%h(g4ԫ 1P r'^zpqۤ507(h kʠ!jP)Xe`kP%WO()=diBZʠ~ ԍ*t^ |ViUh`no. h4̛2  y~DĐ C275kEH괆7hThR >#:eh`eа2]_=;a rn l 6G4ANn>a[3he t"i k GhPHh\rAB<ᄆxPm h@ Dɋ&k!YDBXAZ~ီҌICka { BԕP2(h9353I x >_1042(hh5M)8@Р#8.h|y5 k='d + @@@}042jPBgl |&~^Z:D h4̛2ؗ P8.N{1ZG5i JC[È$ja6 9/;7n>Yi a- 2|Tl tlOXఆ5 1%h* ۻͭ.e7)]:) Nh ٬AtO487dhĥyˉ[(x'P(@a校2L k ,Ə^G_ d B Ea,eC֐Q PРzJewqN$|C}7ҍK47JkokE 2raK8ޛt]5(h'$4Hk nu @CxKӕ hN ܩ&h ep&)h@ Q4l }& te цHkDak( |Bk8a|e"a=;A ?|Ml#V 2pK4x[ELh}3[`nm W\`AVpk`;hta^sa?ACT;o hI4M(h̝4d팄h{F PI,Z3Le h$ V-7DZC1PFkg4@44}РĈA-HeaRoݲrqPe <ç)l4b$A`GPGHI?C[å;5d L ='44jBAV ǿIA kh>]Hd斐*Mh@ y]4Q5iz! Pg!5 ~h2P ΍'gwOejp2 X8 yQ48554Hk%h JP_dr+ nAyCNhhN]ah`_UAզD{$P(@aq>&jL|^Z _GrJP:䲆!1` 3g4 Nhl)|\'8+wn[9W.7Cmɥ ⪧nJȶ AA |5(hxƐm22(h /mj7AZ/ lw(^:\7 ï6 h4̹2 S5@0ц07XkprY[RP34qaNAnhw;\7In>a  ۖi04HnŋW/{\nm>~V.4p 碅_ꁭAudk;޽> ghhgnXJrA~}@wꁔdh 5I裂`K'P(@aҋ&Z[6+CUsCCZ!ZWR(kE[ø1֐=q01n y 152ƄFjɀ5ح(d#b /'&pơ Xx%|@o (մlptdkho [6֛_ϯ\KhAƛOI@`P(kgQРh]NhjP@,XBC%y ZC9W2Zk s Db}! Z U88AAs/e_s '4i>? 8HYPwW_mP[C[7/`kh_oH>$9,4lv=;5Hqx;iDɥj8CV248_4 @Cb5 0'@?WhP8f z4#@aA/*x7ؖSi [O)&Р갩nZHHq8[m#Ijus{Hep AO-4P#58Px6㴡R~qvk5pD?4(r. h@ @kMW <\aN!>` C[C1tHM&Q!2԰n024pYnX(z"*qm{i+hpF!AY+K n:a[sY"pыv@dkpOT¡@Jk jgSV 뉆4504P]OS8CkIkPn@P@D 1jNsL[C5hCIh(51a8aLhmS۞ц3*w($4O"'ԃ0&Y 4G/FmXo@?=} 5P V*H֠Ὣ563pnFssWaF843 #ZCc Y!l q,\FXI cY@1xkHщ5g4̅5Рgj aafk 5Avjw oߺ i Mtm *p1 @AܜBa ~"Rx l /vie֠B r)^ABæeA6`ki|#VkHT4P(@ [C#@h7k c l'Q5 @C-}i䔕nsO`᭛D /B NzVⲧ,*kp.^Pr%hp~b)EhzoAAݥWOޓ $&М{Mdǐ֝5՛{x\ă˖xP(@BCyeHrk 1֐O䬢7ح(JJ!W̡ɠB_( 4ueӤ G  3jK 1׽hxi >h[CY R_n! k,M|G"{m({OHhB]=#Р>hh>~]XH,4V=A@ __ # h4 hBkȢ  pAYo%Ek*kak5pP 4JuOv|y޶Pn<ᄆH_3d$2z'|$4ȹ}'h`k/H$p * u#FX<#WX487[9204D@˖5ȟ:P(@@ԕ5dM!sÄAlDц:!ymE‚0 b3Pҧ,^.G-54=nm.ڜ>k`qشŝ{i8K/ Aʎ I*s ۖw %)ϺeAL48uj+P9V^ ZGf)S$rNF~^S;*l74JCCƛxkiM1ʡ!!ആ"LYm h(@ Ł>EvI@@zܹ>^7wdeP A)AryPL45 rLZP]@9q%xJg)_ $04zKs9vi}  2Ww{>,45Xh`k(?P/{OڣԶiA_RgDUʖkMA/ 00c04 wRAضB۲lE1]kYB;V` c sj ~N9v ā'+#y(qPm&rb~qsv*oKhz@B,dk h`w dIhP9 t#ԪSdCbPHh/cSXㄵPl +$)CH28 k*b '4Hny2n(a&2dϲ5 2CC5 ChԭB> CXCf&h. OHn81[ӄ'/Hn :^4V=] ~N$45pj S_cH  g=ajDh-9X=X'f hP5|РA6S`eH4Xe7C4G=P(@+43[?L a,tpZys=e  5:[O^k }.:b)Av2Id+4AB[3p9 jprSRi ΋xB.`k eCw[PzٶS-6C%ZA|U$AuBAh@ 3 3 yfRNАl sCat-+)|0{3yn k [V/iĠ<[`VSA04 , vOj"1y`AmՁƐ(a R\P@5T'rDtYGf/(h0K3P}WOD thGm%kx#6CqX1>vAta3@07Ɯ k@wȡαAB Wy?7oNI'ly= AzjРMHh e+ J$4}O%4HkAA4W.4Pـ\&ZaE2  Cg%ZÜDF5pM|`!` 3iSyX7hx[vsho!hx#[$78A.pwm0@9}︗Q458[6} 5XY |A)-45Hn(do޲Vi Nh y +ËV>]-4[C&hh UC5 TlBYj@@ʠa:WWAQ]r"[go?ku&̇yWX*ZdnpB椃(\6ahCi7x2z@rۙn](X s tEmyBKO 42>hP28&m LGa A 2&*TN02􇆒[ìrC$:8,l yPa kFjPQ!>͞_Gr e@ 9O7/ɷla \$k!pҐ \$.fYJz=CP(@ädk:!jPy0:(k<调i}-4 _ ZXj ]-g8hE]~woC9kHe^*h`e`h eP ;_ȋ\ְ[MSglċBlT: ;' l}hN<8AHbps][,|_  %=45Anu.QEỤ]7Կj/ӏ\}U^᧯C÷J4[OcA sg2@C%+Chn Nt-䗨Gwjx4#J<:a>MPXsWah eh憵^@ A65~w` k T/~Vu <ٱG柼xG3|eŜ AʂBsֽkxv  ܴRiS C |ĻIS R8U?RO]ONV=T٦ỷ-e$P(@ 3 1h%k7n` 2.j(#[Ct@C 0& U(sWA!s.TI jO&μ9K {i r'p wNeРAf Vk ԗdZl ل /kqKhP *& ͉+/-AwT  (a~!rфSꇆ5T i02daD/sle+4h á6hB>>de h@uSZ0BW'o9y'5O&h&YSo~/l/͐'4*!AZ_GI rR;` | bk AB.զAAzdYtBCsⓗfSP@gRRR4]'X#4$4 gS2Be #*$aΣ >h]je3P^eH+"V& jMHh&|M%B6`hx5<[|+& QS %o.P[@1:ܻ{j!A-304J 45Xh eaw'ZЀB& MDVP'4tـa!2`N_ UYCt0h+W UALy *1Hhލ>X;V9t^`]e5WYf15ڔa8k73Tk a6a8$Mڕ!;aW,Kb`wP@_3eV|cy4Аah $v{ft֑!'AZ58zdpa݇^Uo=e~x^i7.D ΍'B_D epB3`Ÿ) 0-hh"dkaf!k h֠Je`hP5x-tp3}'/:uBC8`AΐeCM1,IDATPq~'1pi2·Iđ 4A`AI\KB& (Ch@']=7,1458! +G/ͭHh5E *4L (CEACaL\!=̘5T eP9aDk&g -3 3 401HwPݛ_ h`kP #2m/i504I 5З~E]K= KFk -V=/8+45}+WV].:k;GG/dlA)4$4B^erk[A5à05T 1QaPe5`]_TB-`a-`A*[xB*NRYZB qAg_Uo{y_?@ƴ`H;(kP@ 23Y҉>^_Z_:@аy-425_{ 24Heh3  0hsD%馧 iV4ؕt @Äq@C:45|( %5ɁPF b]!" 1nt*C)C×D H|`B+66`h#РVL@P(@LBX0!hn坠a05 6Vq(l ea&3fGuӷ3 ?m랶*n+Yxы4ӗ=e%G-҈ l 1(krߍ`BS| TABUZUOm@ʠA>2\ |I/3WaABN(arP2h i7Z585 |( Zø5mh]XQAC0zP;4lp |6y R)vM6|,^. d tιfu^x"=%40+2۫b2X5l L.d_"MKb`2:FXbpBSAFC $7]4|` P(@ 7on!{aa,nHVhC PJ*Cր6(kp xuS@ 2-6 }N 85WfSy0DZ/n*1pq~A=ܦ#WG;WKfK',4,}.W?JP L,@ sg]I8Cyhy[UYC`XCCh sC8CBs4RH@XR'7@04E4HnP @`Aj5Vm+}>{hv@ 04 B֐G$7̕5 E!y%Š04f  YcR9p.LO>r+ ^ S|ۢk$ REFWށ^rF$4pMe1=CCu[B)C+4|f4'>wnR rM28ҧ.  0!hE!vDkC Ε5 )Ch02Te 5 q8I hN䉱܇o?4ND^܌⠢ѝZ[& kܠv؋t]"yN >k;C\jA`/ϫ/$4АO??k m k+)nYnh x2  h2JBC Jm@6W֐/s0:4Tb 3 JP#у=b :-o1D:?f5ΧqNvFB.kl)Ł'rysy},4oAkh??k'   A)Y,4 (P[15tAZCn1n+/q+7䵆k4zYOp-s#i oA4ӛ[o>W%#$S.P ate/[1&:~jT{2@^:q]*\$Hy|eK_{R+4 2CP(@X 2ʐ1Yn6kǣv@/ UYCm)57Hq$r,kƵߺh5|]?iAx8PY.;rI@S04POJe $w>;#W m IGҬA]`9 a_q҉fNu2VSh_([eYK H 2G:l:~C *DN+Cm;k NkH4XnPP2ڐꁆq@ k(^x.՗A\AACs‹&2Ah@ PA2R5!l qC q C1k[C &jV@C5|ݾGN~f]C}[}M@K4[rAvL]gQXf;lcH*i $A:q lϛµ{<۫p+jpK $T d 1 AʡPPmk\PX@Cb ` rC 8C|: mc V5GА3pZ]pY X|@;{f gg$ew +Br|CFuƦRB䆛O t1~da/E\@r$V^%!828n9P@ h2 a 啡5q?4 5[CO04sCBahkJZMp,:|<  ͉\hW @}J|^L~nS tTyRHX (ÜBh̞HqPK'd2͐[N(e A>I$4um4.`!^:28Tb"P(@C%hfo:Aq/ +)!2ڐ` *CIk(g_ghȖk@Ci@c @}jYM:04ܻ޴_APmx9hGBOk@=h/>h`e h/8`n]A7ePР!20|h@ o( +C5 ACޫ:h$4 'rkPX~  ]VrZy5 h(g R4C[l\^] u*Vp6+0yJh`k@}O 5/>'1204țf䥻բ Yx×or4h |2W2FbM&ZBPbqVD:Yø05 孡-vP5078 ЇZc>k wahkpBC8a&jGy@RVuSiЧюT*|~k{XֿlV м]YW9# >hPK'/ߴD0h8"$1Љ5P! j͐ 5(0Аvs!;7LT!zrCuYᬡXa& Wl HePA՝Nc-4DZTa k@0>4P}ݶqFsgnghhN  NjXlA/^L  D 4d>2(hȢ Pу+;8uD 65AY|204}[H%RBO^MPZ4ЀBQBCUʐƢfec =/_́bfa k $aWQ-h(hx);iHhp*C-He h`e h`bT%AsÂʀC= "Գ}AAR}w_L42ΰРvlepBꋦ dаcIʩeD&& 4  U5T aPh(c cAC>uZTngarАnPN4P5n w+C+A]QAdQj@`f2(hPĥ.z*=b++2Pن*jj?4 !P5 ȃ7,BZoq5KАnP1 ǗZp@U{^G^yxRU:S &M˅X$4He`kul Z{R.75 hHde '&hb ahkx=! 7Te 6 2pzVw0>΁XD!5Tg44|] +j`~!~p. 3I*eD0Tn E-CoT1O{o^wA^5uNDP(@C8CWeACQ@%*W>P ! 7Tb ]-4#j$,П2i qX7hȣ +&r 4$8AP5?L5hmPK'J F #XhPyLR>eX[Cq>Ȓh}DW43tfBi TΩ_Ҡ2  ٯ35:3He`AC2:4Xq eo nPh+/T'?fk3z± ܃6y:/3 Nh}2,[E|֠t* xCV`hk(X% 9!;>2aNBʧ7.P(@CEJ CC1k( CX@r " NkҬ!8 4J y֑)}p. R6:m鐕A j V䤬}2>hP>2o޲Lî`bx'N ʀBRʭŁ>kx>4>цqn6 B'tm f& E 8Yi"W'WߺuGNS>|4Ylz_/񖮒`A.ྏ UhMje|a  А}DSylr՛C$ yaН,[Cn(o *X7_:s!g43Hhq߳V喵&LJ /örp:|aA[X4@Cmoa 4WC.urL.޴  (a\ei azFT\P`Ї [CuahN]$c!el(C5 h(gSj9+)V<4țOJeXQ[аTub"g] {A*V&% hHN'.QjU$4TyS¸АA!Y3`wj !&6) )kNr9?Сԛtc Kh[A7nYVLAа=Q[ 5؄Y_TgV>~ 2кU)+CoB6iX:r(ate|Ǔf CFWP@4n(c ] А >_rEU!4dRܝ]JHhPׁXan`bʠB&Y؜hO` hgBF7=5G Sxnk 2Π@j}178vΗC?q}/$AP>`d0_ rA_R';tT bGo~ NbԠ!`F{ ,]9WeAZ؇.EC)"XbA%U )P8Cen*kh 5 #ZCL\3PH;{ZCn  Xàe X7hԜDA| ޛqmmFsnX.PPx 66Π'`MD_,!evrRKO1{D *È0{:ݡU!&ڐlMs *`M hH'a) 5(e`™.49qơ 4:5P8|M{[V2@Tb P-h9䉩7Fs`2KBA)}AB4_422 }nBk?:?ꇆqzZ/;?s@4ʕqm7kH[Aրu `Ktd * M8'贳-+oK8*h*jAAss$ 􍟻jz=H֕HhPA [~# hgȫ 孡mUb _!@ 5(C >֐+@o!zB[YC>Tg4LRhK(kseLSĩ(B5XRνOp^4 v&h:X_`ePM_O^$w  (aPrX| >f4'~hNЌZ塡eܐf ]-* 4ѡ+7 h k8!x뗔5ejBS5 hN<2p"ݾ, 53sDkPw٠?KC>Se v݄m)Az*pQك @CCkx?hFLAA+A<_ m UAC=2  NnHFxnH=aKf n@m4X҄SNus+bK4Ȗ~<׬*Vad]˷Ex݄45_=/\GYC@䎘rӊ Q(@C8È09h(` _/A204W| )CU6Й} WOXnP֐1֐ L 4¡ٰ& V >h UzYH9}<.6|}# oCR9 :YΔXxkL&۠! !@~w%u˲_F 0!e(` C\`q}QoNg`esv eBe';GWn@/C>цxhcbP׀8h"AA[ oG津5'ɏC}㱿{078õmzd R͈1" 2 T3H+[iHk4P!M Bà_I[ @`!` \>eG%G-_x4^m4&3ˆ*}q9׾`G3V^j2E3 RM=VnX @[]l1P\)p0dkB8Je V9fBP'48!` AqC8XC<7J '&m @ԕa^N8KZZ=+>om>q[iWȵ?%-̂D5Hh kPe@5daAޱEQw/M@vy*1cD P2  eZ?d /$D3WfW<)[噁5+C֠D Zš'4$pC,7Xk % (h-xݞ/rc}UO]N- X.ыw79vse 8N,.:rXG*OI@ 3Wi@V& *{ h9elj a\pF:UgCUI Ne] k NexCR| ArNu& AY_;If4'8tG뷟sB3֞OX8 o)He=xvv(Va$DQ9! T 2 w$\`n!y˲_V+0]D X2䅆2ೆ3yM85!x4oF›[l 5HhXC `S$@COnP1db ӲTM>4pъ@mo،1 hP s!#7Th5u# I4XhzsӶ]mѠy/ 2p%<|K3k~?~+ ur&ძX Cbg_+AZh* 6\cu3pAZ\7` 4d5S%˭ʐ> t`1ࣄdhn \T##7Xt/HpaM>4PcVh(\]k\Wkm7)!AM_C`=BP~ |'4He`kOi Vy˲saЀB<4UWGWP`?K_Gpa?-P@;!pc6"G p8V$sCahh wLʭAB (h`k g$g8 w־+bpB7 n)|U 1D df 45g_G$4TgP2ʠҝ! xqxq NHh䆘Cs4oݿ'4Tb D9`iHb@C x9 I tN28" c?W^Kv YɗnmhCmY劉/vtSp+k@bMXeCøve  >eَYdh4 #1А,1<2b4sCk >hTY}atk @Cn`\``^G:Al!r; 44uӷč 4zolF&d§ f#3(~S]H?_@A AY6ʼn < __%Nh`bJB4tRP|ЩC_P4+C'hy<27ޝ3\:ACqdhH 8g+7b,4tZK?afdq& =i'-01(_8q[OQNyNt]d ܣA)tؼ:A` Y&$4HkP`WOp+5045f&i j;^LjVAB'eH;ddhȈ >e23tR4/_p!!" 8dA'>8! 4t 2He5 hx8AOZpepmADh, gFBC5"el *M\ 3Hh-}}:0D !4 yP!| CCrҁueMw CC CCxEဃJr][!>@MXߌ\05 hNe877~a l.4U4_H  l X.i8"bP S . R|[WHbePC3$(ChHvCBq4g q514=07 UhC;$Է8;GF,S2BCIk @ØrRBYûE I)5 S3_Db>_H& TFY_/uaAl}Kِ͈|1V K<  }ahw e|! 5@CBAN}ZF*C42Q!l _|ܐyNnpCހj̴Cko?c͘5r (qw |gfemk%x jpB 8eh@D"!S|r~MPKR < yo07,1K7.28_ATk@& J XCda_488ra\O?8 8t ;į;h37$4n l 5`"k hٙ;ApB7齚fe hRhCY0" >aAedH 3^7E R 1`w`\`h5 Ve' ! S }M$XCFzH}PGhwh]OP)搶-Eez \Ih`k`hhֵ >kK' j!Lh'R ͉f߭bT j j ATh4 3 =fk% NhgXFtȻh 45M3ZCOzH 8t҇,,c!tIם),7:qCG`NiY Z箽qw)КeP/Z}S|Χ/ @, ݴs l vf07 RV\ dHGݺ`@{ (@aeRgѕNhT4hȵh?4 BNzrhdqs՞3m#L7"!@TL e{53PO?w[ß 4ZٯF|аF 372O]hGh%Ya,4|= C$7JZC|G218I j!2T v b0!hh3dT'4Tb|Hnɬ#fZc_tUAրu)As%קZ4AuW37V4` BW_CѰCh8A0+|Jjc6g7.XEzO3Ԣ ywMrh4 Y}A " BCq  <-? fd _#VVh>iГC$7bnpn6e`e[ v504 }>kK͢;2 O˓5+ PT V2(hXy.4078wwR Tf aАW禿 FZBhPP <;!aܡ/, ']u/V_q_ jWT2PՔA+@m@"h e hx25e`k,h@gȨ @sbPABC8 B)C2*C8!W:/Ќ'!a$)Αs`8tsP o,oOik Ѐ lS @~⣷6Sތg?BJ$C\L>2@@ Nh bސ&%B,^oڃ]'P AP5 ٭aq q4 g+C8|[C["npCr*) 8>#A*C)T!5 h=,Xh`kxwc7l/@a Р5Ч߹md 46^*[]1!# V/[Wy'KV ;(@Tahe @ @Xqa(5d>XhpO2."an@rpSHe( q@C]@ %GstN|pcs |S>@C"-! H4(n)BbJA*Z7oKCZ$P@j02h ae@CP4D*V4XD3deР.Am@!` HC8phU7t9z~s1(e(z5`Fh5e N4]]Kw7Z=,T# Y`AAgt?;s' y|y}ToӗoZAgAZZAYa2 a b ZzF Vƍ38L][a_賩Dd!1;DC'nsC<4!>'ndP5@ 9 ]Y?/P24a8A2ry+C<4|YtvAʠA})-  e4 CBe(l   3ΐ ]! CC's}J07Vtp2ѽ<NP^|0h]ԧJA[I 4{o9ۛa t}יje!/EB 45ؽZ2p_e]󈛖,Ie`bph>{ =a ~h]DAC'e030b!22tAM!FZZD­Fbakb! M= Nhq@C28!NbKv7K 'E)F* 2Xk/uhP֐PxKgֆ2ʠAnPkfhE@CWej 1Iꇆѕ8lчn?KǻCd!yO6y=Y aqsݽG9>@g C5`veM|F$s_tB+YlsCY. maB錌YDBCs}}۩;i%?znt?ް$O uKr4|e?AYMb 5)h( !M!&Ujq"JD2(h(g|ه3ԇhRhDF/O Nn$.prS%]K2(hj X7hl|ZР.Ʋtp3X5A.͘S- ՚Dhx9$44'>|n {nXgUw!tx$?Ur d rN4@P(CyhHVPբ,yp@^>kpCBIrC8t*^.1@j4j *X$4Xk nxc6a kۦq /d " j3W~$42Hh`eP ' ʕ28AZTPj"02 g }!|֐g A*CZ2$CCE d >nHЇ5H:!aaO?VX)tH+ 2JАl }b>k'h28Ai:'-4ÆNzfmѠ]?Չ S8吅# "gAB5IOϋP xфpnH2PC04T4h֐ k7RꉞА}DUq!mHӇx0D녝2YI|^!X2KpAe(COhHF5`FeP A@p=!7AZᪧ.z0uYH h/ 󠃂h" PKr354=/84 ١24e h?q!ABC8Cs_yANem(5sq2(n_鼰uM8$/)C`+$04dNk+5 hEx7qP]l i/BA3048qN3l9O1isyjN,!y˲S$42HhBU -gr)C24Hn ͨ!`A)CBU,4HeAC'eAChC.} 6hE'=C +LPp!MȘ4TD rL9 %  |Vntdꩋ4HB y%- Ԕ :S,45WԓB~(N = G{%95Hh k`e`hPrh*tw3|.=-!3*X k'FEi'ΐ А=ڐ 4oч73rVts/ J4KNePTVh0& >kEhʠ|̇mae'e hxi;m#$.HkoB01&T>'էJ`> UȒPZh x%=!2F_FS !2ц}⠺8 u=ACOngG" 6`[? >t k@02(MpB\lFsoЪ ZX'4AZЉY03ޒ~ZB5؀RHeP 7b/PזЗwN01a,q ٕgy:a 5mRdV8C= AC|кoE#SċC$7(qpr8Ƞ5 XBJ yW'7 4\rԢ$RU[csmtd`;2Xk! Œ e2ܦηРR2Hh`A l=SJ*h`bp*C5HqR,H|05ASЪ iq J0Paeakp5wɵ ssDk!ДaB@րuWL8ŁA5kEO?t+ _8qFM{Kn(| L+48C4Tm@+:Xqc V !2j@02|7Қ۟yB%O__RWmo;u'/@@[`f=[%0M%:AۣNtU4He`h"Щ$4X_e@I CWh( ]aPe  5DА ! 4 t:j6zk:Xw_L CWnpj 2D*$MFS~KwjnsS e!-͠xXXw h k8  CüBa_ " h:7˗T耋vԠ~oP>0?`ް2)"\AY+V~/h_d sHjDhC2􇆌 Ŕ4tdޖ>Y(m Mou_B!23my?~nHtXw6v +ܠZ98A)R1 q@Cie'X< s_h#ȭ 4204]s nbΉ>9@/4~IA}@?}oݲ,Ti J42ʀB >bdCSА=ΐE"!2  XC[/tiC 78G22 9ĥiHnkFp@CO֘>Ppʬ%s.ʨɅ{_bkۡ)_/Y4-`28k5(ee@Ƃ1$+Cvh#h( АsHkp( V Neg VT!;4"͵C,:8Ck8K2Ѐu 2ΠAqlpͱh? /ËC;OɽmCJH_+\AX? )|M[ >ep֗oZ:7|e ʀB Đ AC82[C4Hk',7 m#LP!OAAC'eG-%Τ4 7×k-C2bP 4̭24B_[.cmBZ4qQpvT[EuY"wPJ ucp5/N&xÌ[ÈC9Ƞ/>4'$c:aw e`kpK7.1.h͐ Aʂ0oD CCJZC88W 1А >y,7 ڧ y V"!-2 nbhC1Zrj-R*C Ȑ S +CYg;^1@'9|'>z+ R{Uh4/xfOԼ.30n  u'9dAҍK452Xkp.I# UZ!2%102!?wYC|W68C kċq7Xt)C<1 e& AB랶D9;R 8tuvNTBB], ݳ:S@_1 _ZW-&/k +ԋQOpviee֠ABsFQbI <4*52BCe@ܐ` |3*L23!K 46E3!" iЕ 3kTA2z ICn`)N4 Nhk@`1 l K?Vsd ѩ  VxyT1]D@C 1d 2 >q*C5䂆\֠!o-Р!>ΐWbM@CWe( e @M78! J2d2U& %cf\I'.;z?=~/\7#V AA&z%1E ?up˚׊sEjhh_ UAC% SYVkܐ1`!23;A >e1P2058A !20r @9P_Ru5E$kX]AeBʜM=5UA %A/ڡ)EjG6Nޙ[. _З* }D`C@g!6 &R_z`wu]a\h:P4]OVeCm05(Ü6TxxH_$Goee8q[!_Z'f */?a j+JҒV,4(_r@PA1 d0@=4E'( ٭/5 h@ܠS! AX:8(nACe4 nh:?znٵA^qb3ҢӟxG3WOVgAAMeg}.|+7-}WOT!2* _^)_˖h5(__F2Hkʠʂ$P+1PJ:5?k r;5(\d Y Z4 :Bu}a\hnEU'Zc1 с4OAa k @C9hm ?znט*G,\pFn`YOXh(`A$4HkprRI@Cmi Cŧe_:o 6ũ r@~A)6LQ,АF ea\hP~;i 6 3 }+h)ʐgPVHe6` '78P@/ 7>}gAnApơ[5/!;5p 2vsB.EΪ*\bQ.i * AYO!U߼eGL"mŒ P,0:4Xe2`7Z|[Q\Fџ C(/ !>I*Y71t'[9C̊ak4&TGxrZ/~憦9{ ŭo{pY|# l ܣjhN]_ /_n1I# iP^Ʋ2Y@04 ѲΗ 8C2A+rP+4;AF*Cq #ZCϛvzM <4J@G.7i}ʧ.LKN}S5hBA5msBG (2ZGlpXgC9'V3 3  >A.&s5U 2J ^*j1 ! }!/:HY( Clc ,3 |q !oa,h%>q֐Q @CFk( Pe dA)EG,Ґ.=jo:irmw j)5P=a[OՃ$1Hhh>{#_M%"- 4 > _\*h`kPV,1|l/SY`B%,<0 4!4dAQ!eC'kHq22H_ц8CА7PJÊPF CG̢ z V\AKZde`hWT?qCCS/yB<45zö2 _}i6 o9oA394V _&,4Hk AZX$4012&Ne5PB?񠡘5*@֐ c)CU0Ģ Vh~gR2ZCah(ss5Rve(  P9M4,(e`b`eX5eP KZа\곆i=#:lwY>xf4' nah&[:A@ی9 y/i *@gs"˶P) V}qahh.FNP4CJ_HXCd!>@kR2Lr-(9/5Ee 3ƁٚA)T\tf=|O!NfR>|{2pa J7+ý%ʹ+0 1uYFzF9AACsWtoB%gX4ЀBeA0"7 P@ цP402$,7TI@ChC ;\ qA2pA6hIPwU'42(kk)Te z}a扡ϖ")|(TV$) tBr/5ȼ"ہ/}Q1A^P ĠaDn<4Сh2zzZC2$@UpwP+4(@Chà02aj(AW WoiAׇrY%eAB l  TR2J/!eIHH5Hb[i2Ѐ5P]*e 8  i0.4ġzc-y ֠a8TРa30hƪ!8L  lq@C;mn/Av~ \w= ]46t 1֠=(|Heu C@ʐ,(Ck4f)zʦkd 6-_Qf z#`]Nx 4507d ,2P7**{域nӠpA~j T>e| *c!pCFk Cz䦘%遭F"!/Pʐ1ΐ@Cr!; LEt& c Gm&_H+AÉ / wډ`B +\e$ O]g~OzehPHh˗Nڂ S!V" pC.h ZHk(|`eP(NG2|避atk2ĈÈʐl q@CQh`npZ}ǮPzpxYCÙ#I5pӚOB$ 27n_e #e!w`eU/+rN˦P9N ,АK rC|c = ]'4dQet`C)BMf|ŝqZ v9Pkpޓ6)n!| 6m3; DK B1]?| uL _q=2/(hU2W`bӖX۫PL,an}&2 h y]8C5!2BC`ф22^!-ڐfLcNj(a\zflE]6kPzo!e p6;AZÙ´Cj/L7v1aoz='epZ9g#/S,A@h@*C5d8$CC^b|77(4 >N Vk2A Ţ =`6&p\qg4{e <Ӵ 2H_A /u3 0J\W935QB" ׄVxÒr; gpBiPZ{}xe4L Ɓ)C+4 &}U9*Z7'PА! (C1h e⡡5TgȻn-5auWE8BMb~]Tl g :A-l ꏋV ͉oݺ܌*bEۍ6 6ΠB=S) 26 rY>-v &" /Ճ7NhHۜ"(Ck WO8,o =!2! ] rDZ!~e( f8q[Mf_;U5FCmiPgeACPɱ53G^Ro>aK'E.1уr yWxA-ۖ M@' (T!!XN4! ╡<4 Yr}c aV"8T 3JCÇ.mA.9ʁ g=a-?S׽htp >%eF?zPB÷o]vB) _25{ k;6÷nЀBe!P XCkrxk}"sYCd ~rBC^kJzFX:ǑW0:4@ *_KvT Eڴ%x݋v4yiإJq7}a0Զ@G 5He|Z'{>x.;?ZRk {W{C6sU{[1A.q4PnAX04]P5LVh CC ӇN֐}_3+ g4 EM(h!A.{"+KSgdz8v_a6\8 I(e H(k'yIA[5W"C& `aDes,kH]_ˆ߽o 7t8 NhNцAwp VYzР0/`AeOY$ezdf8NG(O4XiB;!M,4|{9cAujptB]"CqA+M4CUqV_SyWXbK[AYÃb ? y UA0>!mIBe#`& bS NnucVH4ąsV '«EAtN:Tk)hJ B;jgHah`b`_ T l 6!w,p&ʀ-QakSHhAj#0457䵆Ve$Π2i 15ڠfԝvvV(ߢ8(h @,C/~МEt 4bA&o3%2(hM _3.v˫&T!?ݼO^+X"0+ڐA'%hJ!5bА놤#HhP @U&nbĖ S9av܈i @Ì+CY@!οEeꩋ͐HkԲy)~x#Bà=Dž1o) {֞E 4|K'8>j ,4>I 5Р(CFhx=.[i !`  V,4(eAC8CWhJ!Yla>Yaܭ=c#BPnC9E =iSթ Iv/,4He(f Cl8 \e2{{\ְm-AmqJ"V'7/.};^UَЀB4 Y o2 /,4(k5W)B XC>̳, h4,4(n m%hSw6tB]-+gyZ0}Z$ ѩ {2R5m}s4x &P05tnF=ܐeCFR  Nk+pq'4$(C24̤507Xw4e q h @l*Nz9Ti 6Y 2X_t|9qa,b |  R$4(k٬N=Y /|=j4g~%8pyCL lZAvd# (LB$ ڠ!5d_.! Nk!90unЪ OBO4 5(em p֕ p .3 KbY߿r9{`qaDb>%F,45nvWnZ"h&ǡS5Hbp6MP3 Рa֐]BC]3;YC+7{!j) $43X'&yh4̬2045P20129 L+ÅG|t;T}ԴP+CCr5L z̫ &ze[z¿O0+ L 8ߖ.9\7e@f |qX02du E 7DZCr!;4 nb+tgt*0 `cd hЧ| XT@Cm~S~?;-[hp*\7 Hh.MPZROO] P÷ۮA$ 9%PŠ5!F 8A02$[Cba88CFh5/hhqCf b58$p5eHXF7! kP;]~V{IhMh[Cs2>e}za 4lAr]@Iv*OXh8sN[@sÅz!2pD ڊݵm~@9xlJT!AqeC+:CWh(pC+C5@ hC58!` q CCT g:~߽ TR>]-Yz Au|ѣ:d lx_l&q6bP> m2O+/?qnjeqhA6` է%!L U)C"JBP5?0it(;h( Nk`n&n>a)C4g@w4$qР.̗W sco ".1cR\Y|A*[i NqpZڮUE$ %   h\zaCQNPre68! }AACmq!ʎ3-t(ywK-' (k b!A+o{I A~C_vфT?f. ZkJ("'p&' K_~O3ܰԌMU‹2T}B ?4$Z@HA %կ!,7jkِHtSAy' P@P@/p SKZ)A-lS ]lZOA%@-4e5y^d Nhh4n*)ޏ(`Їz&2"CA( 1P25Xn204dQ2z @\3: zH~Ѐ82J5kml N Hp+zACSo9eg4HhP JHhhٿk( %\ 9R4) Vo|͉o޲niAqZ}1jPhI #1  P^PrNo Z$4 Ša\k((pU@rh( ]:A'n4h8I  l  zcAMÞ-5'P[# 8{_ \'#u_:~V|/ U  ;1  aJ09b/d Nk`n zJBXPFK~4̔2ȍ 32pы Ne`AޜW;\zmޗzJh`kr`Č)P`9cʠ)P}=]}wKrZ =gMPJ!"` b(c }JK5LW2Zsz_ѯJ*C CuC[yX7h8<M  N_x V x {q jriLCM͐)'18W_Ԍ_ā d>hPBjnh僡 4 88Aa\e`hr5HnH5a& qH1ty+Yh)e h /^Gh{ڶfi)G-^!򭫸p65h hcz2uB3o>yG3;YȎ ?Xp*hUkWt.`b@֠NhX KC]SF5 U4rCahH8T o (m(f ZC \pCCB@045^ZAb2GR˞7 X h`kP`}*AT,4Hk`h`k nq'K r) 2`}<^?CC`A+fhpՄ0 4t 8XS(e5Qk( hUVe h(ø05Tf%~GP,h8aXeР ex35Cr2HhpǠGo* l fuSnJʏ`Xl}4ӟ\߱ էb_bhOl @{7/Y+j*e a\h 8(Y|/+[bKu{䘮>e !t_o2BCkath۲}1Dء&#@Ì@YA2PABÕO]Y@q/EXrv]|Q^4!K.lBg䎹ʟWR2XI!AB ~+7-A~2;] 5Ehc걆pABe ahX ])]Fo 4 Uj\ц S9>]-pBM& rwuSIBMK_8s34Hk|K)H>fLE8"7ekP# ҉wi-;Za]_ 5uhP@! B 4hC7T)BCT! gc ODv4f| ~dbJR d !k/?zA^yS>n$`V_yYtIQ{ܹW 5(nu K4xÒ'=01Hev{͍,@ȗKBM2].pЦ pøթѡ4t804tm0yԏ9`b;k۠h n '?e` ERZ"qsKk`hx$}dePq6CB+7A3 CY:h5h2pEV rʠ831X=n),R3]B=TM47bHV_/Vwа*d5\C507^1HhP 4;ïT«$:AChC% Nh!ykbAZ ,֠!R_7aka6Z63nhd{|پΩ NhP߭/s. ,.>rBm~lݴtBZʉZ=w [B2 VN&d̝rHkhzO3hk@j_цCuSˆ"[݇ΑC;  bI\- @C/klUul Nh7g #!YDAC_ d\_΁A =& wy1^:jpZ71pZhH0tbцѡ! Nh1F0C5@oݿ5ܯ@I T u*|\ 0_A+M{404t ^%A3:#:l# A@ᇯm y}pk沆# .%4ԃRYf Mt@5T ]o6ġ<hVtZ3]ER+^VHp 6 R͐)%>a_Q7(kX% j $748N 5۫49D"rфi}րB!!6]dϭ[Cr fV|65( knY8'dSwFCf8zAe.|>[T4h`npZ:g݇4G 3O aeh%M7 Rv$4|]4Xh|m)21D-" d haA 4D3h@ EAC'bHwj54/ߌ,|:!QMT  4c+O504Hnj5Z$_RրB94d6A4h(m P'7HkhNt憼Мs0Ek~H9Tx Ǔ Ѐ* 7~\^4K} r>iAugP oUݮzBy 2-$BeS Ae/F_R73$l!Oh t/_^1'P9цA!ҠahCM A=02L7PA& S(aeA% ܐChmȞi 5pYe ܐf  =a\n;)M'[gP@\(q>j'V 2`Md o?m'Ysb#}@gKv5C i!|PyѮĠA|F Cx3pkk'45[e |I9=ᴆonnĠ|A**ߪʡ!5 2dцcѹn]Bno "ahCU+8jCQ$YЀ .:r8+uOF@ h8I ka ԻА}p30(4䵆Q!ц507DZCCP?4s(sJ'k(%0w`g\/{6ahr8귿ԝ4:1)h fw-4ȹꉍ|]*\CoXє5-S mP[T2104|=_X} $7P}e+pwo[*r?&(Tyh%ACB!%!9044do >nk4.Cb& Y}izk  lo'oxĕ,:A_`s;B f9?(CB6&SCxoKK  >tn9CU{hDAċ6'4=,'4(kENe/ 5.4 ]a àАm( [6 6nak n  Da܅VMPEAUoTG\~")4  %424æyU;sйP9Ͷְ x!4b!W j)*sUȦ6G lܠat7+AXn h Artn]nO_^ \2e@*dq 1 ] eahCny6X3 @aaR2@i;iXq hPʰ'nZGV+iYo=e' ٬! T d h ؗh0}ꉍ9:4HnpCטCa 5~W^8/lHhHk4i %/|Kr ,%+^lA)ûi Ch6CCIe(` ц05 8?|цk`hi 1!( h2f8 d;h I R$4o5.A['AUzZ.opC PS07^pAzw"AP | MAsAPܠVZhoB]A*ȍ*ACk!-( ц05 KE ;QkUe(sxttZCX y;YiЧ/]Vn `h kpfHn:aO^h3 ,8AY՗BY?Ĝ׍naSlB׮zIrl d >RRyxK UI}י~M74!m[(P b a e58 kh: GJh@  MfhhNܱdAB,g`eE  d o=e2@5=6nP@wf6hظgNns V=c,1voTF0=E;ā)@|п~e l xR_`_PL: 5EhCyhD02pah(m}C5 A5kpZ6n  _X`& l`@Ӭ d M+ÕO];Z' e@p>w|;Hny[V;Gm<)޿uG598Vxp]s~ 'ص!:,; j gV vF 9_q704Hkoƾ?]Hj`aDkPK {mkU6r|ޘ/1&1$u(*(J%RH%PD$&jLi{v8v]g>x[w{m^\Ŝ7G"0Ch: 8԰mHXkֳ2t$M ZCq irl!- jq! AeFT6GdB[C GKXD3ЁwU 1e`\P߇R d5044b4\=gKhޢ fN `aЛ5 _EkP` ZC+FEcE794 !b 8@ 2sA: QQי A]M޴]%dtj(h?;1aΡ'2 /qTNpӄeWkD:{?:!:g[x_:1هS2BuÆ ,4ZOs>o"n@AAl'C(l}e^+prW2xy74NDGJ!gezqiAaP0[hhq!25584ts`Sh8CzUݚ(O_TР# kPP9< BA}<|zFC"# b9~B~}R %c c=A3 1kP0iʩO5OЀu280k9ִHCUk@! ñWb٧2?7ȇ @ą[Oo?tʅ Yyɋ wr!\ACg/U+?{^5zَ?]Ͳw Ⱦ\i>HOde:cd@84T:b=XC+k ЀhC~fF6b  2_7YP o¡24wj&h(w,C-(hgD}XI3[GM*pݨ*[ClCfҡ4aܠ.(hP{<`Ё%cРOUsx_X.~cmWr   ^+zǢ( qKh:c>uQ C\wv$,>:*a1O @!e(&2Ne kIJ1hGkD>yo-1Xh5h # /cր8/(FXŅ5,xyhAC^Aqg4? ͹kHܓZk`n Uqz}o`ӳ-o¡aBC =cՙP@OUtpvȻ_V@X S-4 qhumN~Y8bSCp(C&4@5d)@[f+3!f7$A* 0ØCLXa  4XePOLT \*V)zقog^8C {Ĕ;Ah ߾r'^7ov4"\CWNJcʐ+_xDy/S,7BsC 4D%ˡ!/4Rhy7h,4(kBaP(}i _~ 407ԶW8̄zWu 8O)C VB 8CK?(hx%YA/𘆏}A᎓'F']7L4q >2 @h>܄2nE V|XD  3407P=;dmhP(@CkvMurhJ )C~sDCh7 !h 8pdC nP t& Б584\ =OthphXTs(.d3@# #~f4ȆSA3q" Ѐ#n>aa*!I  |S/~FӰqR!֬?*_8`V24|ҕ!QbD5~@xɫ94xy94Jt B ht!m 1S!7275 ڐi ` kphhr*n   ^B# {RЀY T?(l ;m1 732`EI!ʀ:`!1Aa_1 Le(i!|e$ :/! V|G9zɌ+|2}p5AwhrhdhWZz'7t DaXCN52jJ!.w])CGF6@nP ۥW xA^B8P ~Ϝcx , 72 d!ΥWE[5`{R.04Lug__^3120-"  _7wWKwȂ5P64 P|4CC Cshh9 k:6 ikIaP֐Pj ?E֠ klm)ӡCrbkP} h`ne/Ġa544zXqihm:0AnH[C2(Tƶ5A̡aVݑ  n  l =h2@s!_ِ 'kW~bq \ =e1W"px$PР"נuC pC6M'G|;e u CÏ>+8 û2`b}7khr8E%5"Л5tpXP N kn507AN`naV8@g_V44X 'lAA>'/ЀTRZŭ>^_|D4:1Is>5bo aG<~窝=Pw&Ndk`h5`  o͠[ ^^]r.4H!&J: 8 .h iehh 65hrCfᷞ}P<0sh::r•+mcb(^ ~ 4Xe?LVAAe>4AihX9bk8B&4ʥ\v|1hHTX,4[އi7| Đ//e8Y^WqET!z 8 W=5BC BCw0Л5AAr Y?hYCȆ|khmCН5$&*4|%XrA&IG>R%~{TH:leP0న1A 55@Р 2xꄍ3)h ZePo\`h(_+//DDUh,4!Fn z(4MԆk@E֐9! ]Xú9tn ]9} ^=e2dhe5(n4pƒg/=xv@d! AePp۷)kP {Υ\xyޭ+/Ϲe"_k(^uuKj4Cx*@ lΕA^ڠրk!4dZ! CV7n(I/R9:ehPqװm5Ģ Zskob>w EJXA Aø{^k l :ړYkPճ9P s.W/@5(h`e5Ĕ(#Wn5U, |A)|,nhP֠./:Rk8B7f+ c/[ 8td 9֠~,4<k"ڰb]EmYC\*' ehx;w$ <R]V{O_D x$tOT`ӄW(NQOGg(o[504Be 7|yj)AYs֠  ^^`aЖ2t "ͭhCUnh}{c^{D^w6>e &{B.5 rw/i)$riAAAꞰXXhJ$n!a j@CQx>f ACG%kJmYC=exƯl BCk:!mP ܠ!m7G̡ϼ584Yg4yakB9pI|hPGNTH:ck(.> c[@b # wB,K'P 8S ihx(AA*AZeKD`9gs3(h֡q(ڂ!?2BCq!}P[mj D gZ?r&fdphOhӿ֠F3H5$WƠx հJ`C #ڞgiB"Ka P)\*ji2ʠ !k_4rQ>wIAy˲\94Xkf-kA(п8ik 84`鿼kh@'Ѕ5Xh\̡ϰ5t 3 284-4` Ak2Vhd)w58e !}ja@je8wa5v +v L B(Cʅ?D)hPk p ;v(h8b97xy942uCRt8Տ;4ᆪ[KE0+k( 80~L2RqLkhHqO< 5!f L>zD9PRXAP 204u.V5['xɝjm71hkBClXCP@a@&Aɥ#'2A 71Xex &VrZe445 3)U$K^o.VGPjCCva֐8[C8AZ( ! _e e *FX1Ϥ5Xh}mphX3|PlA`! Vʐ nP>_ `  EuΊ5}bMVA*:A bj8= Ak#οmYAA冪XCp$C50JE0242(hٜ7|y,yk/N, (Q7qmS'M _~xKeXyyvWk.AOhky7cGyۋ6'V$aš7h5ܯ=s2ԃ? LkPYRkPк5p)k'C sl 9&ZC<ΰv :1,C DOĒkӻO~7& ڑJ϶lq3?UK|̧Ao(u_}k 3\bُODC(C1A! AexWϤghk,|hoz7u]n`h`n(#։58Ch p@'o8e s Ɗ gcm ! b G3N41.04`$چ%P%U\t4w5Ȧg%bА =φZ(C l N,&y e4p e*1<hpkrhAxA-rCZV<7h52tg > kxڈ:V5Q5p5hC+YAYC X[A9bxѓ=kO24I4C(<,2Գ ` #)րJ2HXApBAAY  o`XD)(hpnchҲP7XtA hY21b.!SZ3tm 6Ѕ5HF6XnYp~^6Ƴa?lKAf /a z; hP7- 1Rj4maVd! ц ke ?v*hʠ1ꘉ4p߄@.Db5Ѕ5*)sCFܐ 8t C?^2g`hhb zi5p~{#[ 8461b5phXA~tPAXÈ34邵 45Lc힀5045pZ`+D~!Đ aN O/b11 e8OpЦ=3hhn d2$ -Cmn 8>VЩ5n8ŠEO 5(k4YCwl *ڐ'ށ%g[3NamYġ#'\f YzSN~ 4@P, ^NCC@*9^.=i :pE <|dc┕8gc5Pj c (C=nv C܅8n J4t43R Н5~ ` x5rڵاz(2Г2ƞMzaS~V{Ѐ`khTfާN?+(CPbpob`hH(;ԡ:g;S1XhP4i hhzdeJءZ8{ZJ!z.DZ6Nu  284A gh` h8k *Ԁ @sgܧe L_oyfY|k~ѡ-nHCš]h" *CGqQ?ljνuX*Ap];P;S:l֐m)Lmb5#/ At( n(!uUdT~O>3 ˹k&`W[c ꐧ$KAVqmjZDsCТ8tԎQ{XV M2ĬjE5s+PXCm ZWa:T n߁ .w'C@!(. 5=^2$6̥o^z}5k+Fx 1Vvte25I,Ϛ ' } P 4 p6jFJݡghh8ġ-jC%knAA][C+Q#hXC: aц! gmbphhd"'.}XK‰[yWAp5{m dAM/*nt cpYOe JAPe$<ޕ 97xL Ѩ80˳aP2,4587xy Dj[C&57Сg}p1Lk V2 hmuk6FِцqCghրl Wv m~eh8e4쾝b PןM5P{AhHP:0x깈tWcT¶>i4 Ѱdq>fB125<_'/BCUzj i_(7Car|&YUz5q J!G32@CpXcr5(fuWdn`kFQmkZ'EOfP}CS wj{Kbn-V^qkHC~r#J7.>,SC+|?>͎TEy?畩#֡S]h1Xe/ZyLAeA↢o߹kmAC)=a PZ _JClu$ q2֐PPYaKQ ]*ACfh 5!gP?r Ae8鿼eXsq7!Fn`!h ]Dz9'4Ԙn&jfH  ?Lq2<QE =1ФM ^DC yҶ*tw6ASUk[VuBvYeeK l AhPo~A醯|4T/Է;_$hXU}7"CXC_ȇ7*qC:pzцvš5sACBZoAC+q28Ch6( khK?Gkh P24? {K` Ah*#* G ؊\2Ǜ!Y{ a( RogV{ZXB ` =(h`k rȈ:jqȦ޸64R- kP(pC]v 5oWUhm!*ACy1LkHCCCCkhXz@ Sugȉ6&4֐mYC+п2c  ] _ 184heP+_kmߘ8 ؋ B ˏcPzȆ4H}R+ݥ e`k`hk3 BHb hϱC,4\]l so].nן-xG=nht}𳻊5^ G]P40 e{{/O3ʀoٓsz/АN5TD!3mȁLndъ5Ԏ9 ݡCZ36MbʠAFА3`cc&7$ khI1+hjCzWű +r1L41ĔAĠZL5AV45_ARv B$O@Ap=;5L`8}[V4EZş dhbe>wn/[ ihGG5xyՀRk2+YC&7$b nWܐvhFDAAC eH@C204$!g`hXWqhpCipCJP | =% PM`1L[r\Hg504^1z߬p{hX9u;4J7cyeĠ߷XX!熶dl3. hk@R{V_oڢāG(W3f 9Ѓ5794$E6 ;n )42E! sg׻EEhC,׀hCm7o10sh( %u +m CK`ögAA"7]hO.đ{K4+C%kxS-2%M\b PVh?m2U'{'X-hCG7 VРz(Gc>`(~e r=R փg/Ŭa&]~* Ae{ IeHCXAbZ6.&4vJk8jfʖMOmРA! AAh04@NfIam _84y_B 4/] V5/ʂN 4+ un\/01XeP:8s 97!V-z D2xh( hC)7ZCfaPЮ5֡aVAaBF$*V0Ѡ}RgLMA)/<#Ū1);kȇ j]kc1hPh`k4(kBsOݾb;蔋Ah~Cš^!6?Y7,hO|E]cM%mr w~++! PIoB-j ^^! Р8XC`w[YYC):6M g&(C%hXqh746XC ZCCxZ!u ijSA!@ P-VGo ~awxWΞb2yF& ܼ,pޭ P(Jw"2H^$8 `!2Ġ!(h`k?,a 97fI ibx(\kPok? p懖43/ DQG'ga{Bh+5pRˡ5Ԉ6 4077RnCr!'PbЊ 3A5t4pJ<:8K@T4])@|A6ɸ`A9'zN[ؖ;ЀAhP7HʇIŞZRoB%@5ȊUA%,42\zN  Bs8v+eGZE`@ˡSkh k`nnJАpw \ZC?5! C3/쫟2C64$aʶ>ZCUh'eN F s eyWcߵTiQ3 ٹHpùꍜl4etRpS6ϊjǝ&&#ԩ\I j@[CXdePаaH24H֐wPI_T}rAK@X3` Ye4C U87x94TP{pC7dC&7Q 5^!-hh( < ~6zd/2AmFbkPP!4(BC~ ЀJ4(hH(2 B] 6SVٶ'M|xYAV)407^?rC]i-2 *֖8yRh`e5XĠAP4^hމԨ+)+h e+tBT$p+~Dj`hkH꤈AX^ eOd^ 5hsC8sC@CC)= ġ!4tUvA! M[aNX<ΐSua;90(wȴ114t+1db7h{ kROԫyN0þVVdh첑 xv·Yx(!h(U4x뵻wZ9 ֠t^VupWv*R1ΰX֐Nz.mX=r ʠ K.e o9,A_P9"pkrhhb sC8T%͹I!3ТAdC:5 q4xqO< U!A!0]a0?_],Xol&;Pu@sC0٠뗰z@0L|zz"C[h2(hH1OO`!b9b|Uk—Gʔ54=Y֐J@ g\$h5¤%!јcs +𭽲rc*kF&buXЃ5T冕/ol iqn8 +?HO$](.@@CL4$h gh 5 00w90quN <C燿T*Y$Ȱ iJ,`Akxݒ,N1ؾ V.NY`cǬ[+[,5AA9f>ˋMl?2rh* nk#1nHC%nk5 5w& qNF̡Z3dS, O[%3H4on7jXyf@:4 kAC271yh"ok46Lr9tV ;ur쓵 uB$?)b4L Փ9DC4p O4242040`߄ ,쫴121/eYkn~?A76EcK,Ȑs$0nne ,j@Uh@Au+045BÛϟlO|"wLP޲?ՀwixxOs 'N8梅c ` l +kxGb;py94԰ l g>acB_2A+~ P UšA}ZC&:443dP)c =FCAXz`סNkwYC%hXP5`-چXA['2@@ myC]J,ѐ(~Y`b(@ih 42dP:QAV朂x7Z8ݰ  {B ! 9PX^5@s\~ QB[e@Xeᩩ ,7!^[4A~q&e CCUkhme 9䆠8rClAܐP 4:@C (C ڍ3Xhw^U!jgެ wZC#'4814O%q|_?>ul@Գra'oSM}` j7pٛ,. *VP3&CZ=⊇w*20407@4AV)Q 59@l7-+h =v"`Ajs y94i 0tŹ!!in4CCN!1*34Tk 4΀ 3 C 90Chh5T 2+bXР~` hP(ĭ&Y+Lqlԫ +,d=$ޅU 4JDACqOZA9<py~! l Ah4Wy [7V*!4Ĭ8ᴫh5D&2 bu_(@ _y6mPMA\H[?brh5nP n8T%1nȱ|qt7Tā'#M3Y]V@C, ?7ʎBC@;(kАа6Vh50İ dt};/I L=[+r2(e104ȚLmyShSZ204 Kg]r&٢204g5'֐ 0Xhl а&& 21IhPK!`AAAKA%xW[K jrY7, 4&9dxhEl:&A% 8t/o GW=}! i `;AY] ihg b 6ѐ;vش5$A! E邂k {bj@ê2H '+ZCа9~+Wo(XU\~+7\?2rhhn Db [, ) JjCCbld wPZ 40 Ah5A! gs;J0|ehK pA3xK@ .>b 6prS`*! mCCPPB:] T߄݀)yaePpgd2_0ePЀUhH+C)4iy6 WЗGiP8bm>SeM+6Eo>RqojAa^,V\~sJAû kP3\a R] pQX+C퓶Q~ᚗCp!  V8sP/PJmACPj }zq27'?$5 Vgh8C#=!jYCP;kkn\k:u-F3&204` $.׀'  }A)ߩ 7L:)>.  (3$%@ h!Q* 'Gxgw2 %n`h8 3He,+;v4$WV?3h`n@7TNZae@^YnGl^ ô*qx5ġ7 MyV[G:Ġ]exIiv@ ,:,Cp= X_`Od͓;+[7a P>@ 4a ?}ldbj@g*ʀ1șx[dzOha J&',2p['}iwXAՖfh;u"A jA 43 9 @a145 ޹Aݒ1h_ Ѐ A_`}h8m#]O|μZ 9 478nxX؟a%;RcJP:B ieBnZ5(՟U ֜2xĜ@Duw^01ȩud 45l!hT@Fy,&b=6  Lc&AY .q5TD2׌ACGLD  Q#l(XAA?˷Аn`hG?ne/ eH$KO?sk vX\9>4e28C[A4b4Ȯ^A8@CЀmh`^)>|JeV.~pqy;pCgAP0ᆷm0H5Dy94 T'~O- nP 2Ėl~%z 4CCHCDzP5PI*MgРi4R~> O 5(hX[Ġ!= r} |BCP9f" 5pĀ9`㇡<ﴫ<:: r#GLVxx +#*K= 4pAmPOAcEDP24MoLB翭mpSh†գ. u 6e (A G ʉ9* b֠ I*vy.ʇ ԰r52ˡa YOc )b␣ V^8ԠC)4aš;hsPPC̀lg2xޓRD!h 9 k,1Tl[ 2CbX/!b]+يW  \1hP+eAdAAsCp?헅lB ++ڨM޻7Ahgڧ >G|/-(P) ^Y ֠²2k05M{` h \W=+a ΣFc24p!ik2? QY/mqvUC h`kHv W}f #'z LN . -ԁ0r%e 0 uЊUןXI*42j,&0dk7xQ V{pȏ9 4t'+-l{P*͛qu /8o ;c: 5aP֐C a$4ߌ,Æ3}AAip)4(ne{ hHLp41h5Gx"Cʐ>⁑,Ȥ} )ʠ& 9{jϡ t%l?/ 00 v.ÅDbQphM2 re4Ώ|.@ 5rN@Q@ZK bky;4 6`8VpA)C hxk7G^ d nCP␃6P5PJ@#]IDAT\4YPIz3Ht&Ѐ1 hXW|bXCФÉaBUEm8oatR! 3)ՁP4@8wA^ mZAp bP}1l gAo;Y!?,ePThe`hPolаat* gPhʑ l;ya TMڮA)p7]}@OB'YD+{?Oc)xj>l\/[h@G>Fy94!kh7(q=fCCGyCܡ2ſY2A)C8 gy 5CCjJ AhZD 'dbau jw|solj*.]2 e! ~1h SP/BLj b]?/[>ݻ,{\2V wݴ} QQ#'lj@<>0֑ R/*Ԑ ` 45(n8['VQRhk3)4{(b\ȱ1ݑ 5JP#PBvj\ӊ>Rh?MPi!g4l3 "4 XB ٮ281Uċ߸[ !1.okȆؔ SSV#H=SS eX@jhg8Al * % wSS촃ЀdwM6jAb[GI.M#@j hv 4 3TD"gʄ5Xh_ 4pA o*`A~0bk4520qqCoBq? |}sB(kꡳ e $&{ qHCjUĮġ5tihȉ3(eȏ3&xM4aghO$!jXP/0phdpb7hec-407@ vd 5/x_4Q cM)% 0V@X+Cz~('=h2b661ĠoVk{Nd@7l u2WZhƸ#kXƿs&8thCU6  bq=Xh5Vgov@jEjDνeX#eh+ &=a|;)jXC"נy4Țckh%Ѐ7SJ! 1k CC8ԆAYCse(Π&<:gLCެ!1@ì 81 K%5B]wB`!HVАh_R@Xjl$Ode4 ΀M ;`;&b0~k{s n@  ֠8B`ͳ(" ikmm;h`k7#~Р-0.Xh(sC%K(@%՘5 ȗ r\+ES`r μ0kP?@j![ȏ9$(!sQ@C2v8LP bPg=E* hu Vb0QmoOk%X!y 1hP&STRRwk{d>!4@Tc 6eNlX=Р脡A(ЀDëOf[h(HCdGeQlheA%  (A?l X BgƜ*6(_?4tFъ5 g81nHj B֡,I2b}gX+`!*dhZ$ }kJ81?4(klWy"~Z O?bKi=E͏|@Cp@k@! z|7}5:@ѧ-pE 8-` !Ѐ,Iڼ4" 0nT^.蠢(6$NZ6b j?z阃9K:Όl!TQj8Ebc_#ЊGBC2Ԁ3ih` la&_!O4b1428bBC1 2LMd0Рg=&2ډ24u J(!YuBWv>yh`ehH zA'DаmT045]7s b2k*0~- ?rdO Zlq'yۏ\c.Zh204pjDA>9RNx;Խde4Ȓw"kPʀ0H"X5𙗮 ^ ֕5m$bVToEmq(ixzC eACBbq8C P;`AYC'1OV91Ihlȏ|cAÒ! `XOٴ_.,y'cXT;eP`'|Ary_+&/~B:e}}hɈSqUN:big8e b gHN 5(e42ĠA^ LP DZzцW_% >|wh~΅VŰ:#Gj@Cfaag24C z5̖z^ 't@)b`eP"^f%<C{H(h֠>R;M)baA}X/ȰB ?I 9aLe8ÜA 5jPC 1hhk_N  1h`kA=RA7K &eݶED'2 nH~defU@ 1h2(k(.g{;24@ ,424@ 9-`h54 (Vi"xuVw8}Sf@D,45_f2TXuǘ/.VTHPpCP/~ q=E2stz ր>KjH1t jXC=h2Ġ CCWw$vL ?yAD`A?ATAg ~۝A[;qXw^khQ4D2$щݿ312PAe`kH@\ם6޺+h&.mR5 EQe4 1m&ںKKVT`A(@p/ ˡ546k U k?& 5!C;T jCzՈKy7i~w:=RР'옆B c0l8zD:=JPG6"`λmY `bP  c fb~?@ͥ!h(X,~C(C?{OqBcB]Vf}be5!Рg.YO~  8xM.JJKeO=X<&Ͼq-[İW\|!SjA(ˡ-k n Cl%W9AU=hvr~q>q~W'24ĠA! 9Rh(^፣W44^ mhP|bPBPVa+g)6ˀY F~/N<%Qu[bրyGb!Ѡ5@ +||4kUϢKRC.ckhX؟PJ4Xh[Kϛx[ԙ ʀDja+ h(Y պ³ (٫| M4[c2Y4 ڠy94h CC%kX2!xb+9萠Џ;4`e8!LB ))hP0h CÜܕƠAL4<} ܧebPsAIXh(ȠA~?4v8ӧZg:DmG3pM+hh`n})AF'`& &viO&  X@~g9@ݲ[XM_#KAd(|֐[IC叺ZБ5dr)_8XUtHC%tChh"a Jj@ -BCi4@b0[nhm fBHH5D p,Ӡ *Ѡ RJE 0Xؿ/5>Pe,7D8`A.@\2xnw6D 301ح)p`F)NjKu.CSР /xe ` V}[^xE Tʠ|A&|YPAPА3?4|XAMmP) 2C_ ȒG]^ CChC֐m?1nu :$ܡ8$C wP\q g"=A!j,43,US04CR8184 hbGB 1`5νeay.R` 졕|tK`"Cؚb"\p2XkXaM]+bU HPS  `OqAh (ë624P+! hDž7PõKg䷬Cbа 79Y@3?$p K zrGw\4MuP 4QXAh@& +։5߄D!+P#3⡉D@ C3Z'fjP\_;ʐ 4k$uiY!  0Q*a9;E" " Jd~!6jlAh['2ᅯAa+??9# * #0bAQ44͚BQ1-gD P@sC앢 Y3!ɫX+/@ʐN 4Xn"7 XCUnpk_3c`!sd#-ID9h8khJ!2T޸v߄CZcCR401(eecǐnPT'2A v|wB,47hUI_bM2AAC0ZAej 俅[oa\%dXv54(khP;(,5&8Q2{{K8R( ^^͡5Ԉ6tg ܰ>F;(C;UI"0~ie(e8C?qc: 5Ĕ4 5& '5c v@p܀:˪ob<$`e`Gl401'2AE8ĠAAAb _.(@ڥG٣Q|4L~OV|Ah~ E{ddd 4H<$ |&($?],>T ,4 km =!p PP ր.4ۗeA kpej 7td ц':f7 qC%h5$bJ%Z36Mx` 5cJC Ah 5¯.4t7F߄U 't)(eyXeŲh8RMd{(_cU14^QeA iB  6 N⡝) 4[q"@b&ARu؄ڙ/Z8v:PkNڦvԴt} G] 7'04d+ be k`h`nW*+ q$j@tz?j MjaC:3 qe4<޲<&l 6 ͡C%h/Ӊa( xuLs[DJ;!vf0دAMdU%AA3L#VAANF8C2ԘxC; M@aheР!x14`MA2Xh5XhM*֐_8BJĈK  XT?Q瞪 |h5Ȉ2क5-/Л5ԃ!x[CQVr@T7g 44 5ԇ)C"7_ C< e4DYeB>@C#6G [2@C0Ѡ2ǽ]XC7mQ |lJ4؆Q{bUts0`gX@A&Ԙ)uP4 O2'ge+04{?BZ&-C7 mJçKD42xy *Q5P]1ZqA;ĬWN,J 9q ggUqO=xyA}W)rAz ?0ODqĽTL 1P?7r ʠ,1~ uǷ21OX ʠa$dLVf+~z4204ȐN@Q֠ J G|ӻD3XkP;K-r%N|+7ŬP~ÙpdQnWq;yAhPc~ _ECA%A;44hC[{?n Cq4ؽgD%wHCi ihӐc 񔍲j(Cw0P70R/9r8 u" r+= A귝/w|k,3Z',7`LoYd# zc0P \(ҳ h^ < 'd!vb3Yvs#X,4`'GZs34Yb nm>G֠Zfb 52xyu mYC74{pkݹC%(u q:`<ҳ' 0[h DC~c/z{5P^Glakx[141 |2]OӁێ{Ѐ2Lc AhL\UuCQ <ˎ Y  :moBP/Gk+C>407(ex;+k^Z 4 O:J&0rUSiQ'G/ޠkȌ64 ZO}Jr%wHCA(wQ8|Z`S e2 U)b0Pzg>p7mk7Ndx 6  r@Vib0&7p2(`w- e W-eB?N@+S}e ~m&b7mZe74pԀ@H24ubd%ɑg\Uzꄺ=,gjkqt[1 ^^Rnh225fb jUCAT3 3dBe vgh8$N  eNT@b/zC)S `3/21`P5AI2pAxc8ZhщgP h 2>m-V^h`kTVćcoRЀ\Wo*㵧umSkP-\7 KAOِ.$ *ѐYϛ++42xyU;Nwr 5!bc2@wȡ @4qa44B ]too_=0 U'PYAC)7(Mpbphhd |`X$7Ć2^RA B 4Ȼ\nlDG>ոGK ‿_–UMlMo]3РQ{z+ =yk)`U}<gDNhWgh%'FP>#kxFu'Ya~f(FW/&<Т5!" p 1zHD;Hq> vO28C^?ΐ8,u ְ׻ėr!oZ,;17qHct5l>XOIL,4ϳ04]=A {"a`eBWx(iW/qoe/J`Hy+@äo" +==0CBbGcaJkP|] / Ѐ Xh@ h2 g$qD,/z04F2;f^9]z@! 4xaА>x*Р!=Cú524<^ ]V^5hb*Cl p=*Caxi`5 e5V x}UhP 42(hCn/`+s[CeFBx@UYmih2(h@:NpDl!XܾXm q!eKe2xy M [Cdrό6ԇzHb&D ` ?'xۮaЀ}o/4{D,4507HrV>rB!g @ bP;ZC 8eM5C\#-U?EXdeQ 0bۖ=c/Z4hA! 5 eQ Row,AVn&P␀(@0(ePր1UAeEd"A8C_*(ȓ._, hx Aex+>R M dI b /_>rh8`!1+4L%k: ܘ|{ͽX9ެ:;(CupWwK \ а39K2.@p,| v (! (V4*/N7̟5P)h~ oBAeo U&f _phphphvoT1 )!t2p~y@AYGEDjh(K7HH~  l +nDNT;2oi4?X,~uwYvXjvU(y?o7oQ=J/9[D/5~?Ar^^_ ooĪph5XyUyaȏWѡy;ٱнJ,4G[k(mRxb z dB S ձ ЀFsoY.A} l$5g324 `PBŠ )9e f_aQן L,e`hNB2<55DAs֠ YCB-zAƛ37Ltj kpH}k`?7 -,o+ ֕٩2t 5"  ޠ`+(#ˎ7)BCZQ|%rʐS hnEʠ`BdQЀ~VW,b5'o1 2AOu,@ Pwt%C'&1R@C2<\ Jkk*7k öp+y֪P B[3f 0D5CmH#X O yZ2"}{  g';G3(C ACs_`hx/pk 4r 衟W8xB 5(neuU  p.ϓcqRtx~ Ņ+i&\Qn,VE4HA%z-3otCCo^/'c7o w-ʒWx!`!SG,E8c;AN(eu𛷲2`<5HhP)C"aC{֠|*C>4H/:EHFڢ& QC%D/ hjpCU2!g4C1JڅV"    ^G5-/:b5`AA?_,n%QѶ-h |7!xj0/HXfR=m0o rQ%D48CpY7.[hPd/^p_NoBAUo(%2XşC!ub3&EwXCZ?5HQa9a.{y4!3Б54pE=kRUBmh_;0¾ۄ/ ~JmqC% iehК5 =~e`hP܀([AV)ClA  {re B X e`hNsojeh`e`hP)H,$+5:Q/]`h'U7A45 eȬ._YNUeh@]tYH4 M:P 42 NATg ie֭ʠ&DV/4T !z6tM&Vփ&1 nh*+Ci" 4ё24|\b˭!N']_-AG㷂,4oT c` V<Y1hHYkӱF+@bhI |RW/ B  8 g@x٦X `ḋ(@1XePQ$Aa [+ qzy J;)ڵ uʼˏ^=ZL _ XPe5@.og=@Q/_cY281xy 4$kgIRT2p 4K6U=pAEC5>; }#XC a!M ۩2CCAm^|k y@Cqwr0 [_@AA+r2|_S|_#k2A rcbvXaucTQ Vl[gmʢNbq׭N=퓥RRiyeH'X D<l[YpS]\ bͭ!gFr|!GڂXlU|ǶֆYWZC+zqACwl!a к2@ì|kk;WX0whL{5Р heeAN\=fb e4PЀmj+Oz F&4 "s, 44|e XM\Zy43'GACPSAభCSbxq|tU}&W/aBݬg X|!7QJ!r& =sC nr{ճki_3h7FCG0C E#fs~D;XC2{c jhUnvcYnP 2l_blۖӓ#XVog(!  o1B-J olePЀ2!SX5'm+(6A^ٳ ^^k(|)nm Ah_K&vhȏ3> hh?az ' nJpϏ>Cnhb 9    [CQ/8۷ܟ 4yv;o(ˣ"204c45yA^*e6q2w۲Xã?T>%^+C"YqǶb{t2g1 VM(y߳`;& 9ʠ732CR{bO&'/ 525|Sef-BCh(C~\BL>Lyahbphphph j~[X+۪a;?ykOW#!O57(the`h`kkS q%! l |! (C>yO;8!h 9'SwJ518C5M8+C:h(^{xt\bqD~A~ ae. vHJ [orbZWР U?3Ю2t xDQ ŭZD$@25 r0$L;k99CCCCk ܎n'nS;XXymckbD p 係 Oh?zZhkPl2Dd5| ь44L2047Hn|@̃T | 2Xh`k2V~Nq|e@Tǖ7<ɝpFC( 8H29}2+A^ G'DZh@Bk$ȁn'4bhې;`B5(e@T*γDV S3d 1ex[e)eS,(LOl 7qWN2f CoP?icU  [àAqCo0hC&7848484tٯ"CQ:_ApGMV` 1.Ȋ'201䥞\R!Ed'̭`Р" v.s hxYzHn8RABZ ]*C `2 h2p`! #e?@ﰚXCܐnЊ2T3!7pq 4h9 ,|&&<5&Ѐ֮0A'lm.{AAYKp=[Q= 0o+o9rz<$ѶDDS }2;+S& -qtI/ +oVD!6![r*Y:.xy%4e ]hHCme &JtLeBCpޙ+ bIC\*C04k:m4P<skI @[@C8oAYHȌ-ӧK$WOCCN'CQN0 0qRe]qvnezy14Tƅ"sNhK&:3CC2CC,qyР! Y|+Cos!аB! v=XAmOc !NU Y5`<25J5AqƁ[P ː/OaA <@Ğ=1 2LbY4d8}@,/u 7,45TY)Cg\phX}A:Q>ahiB[CCskv)^X\XJl=[qnP͠\o 6R eW.Seg;9VcZda<$'\NXe`h8{JAksYS#?XjDڲ|h)CP2BC=eX}@ & pCoц:7ϛphphphXPijl8P@U1kKuI#' 9mpFCZPieP!\X 1hPgOpxB։ ;2v%Zl P>m˫ 4pM1Tq>!38*4gX?PCzV&TAqCO52a? hk7. jr>Hs 1'b8BP|U;&S| 4,xyy &qLehiWnAà Qz̡x~Р`Ππa! n    YY<) k2pCO p`tM`ٖrsFk`e8f@c&$Kg//!1'AlaP0 *4754M sCцP4v\s*kP [{WEOmH0Y j\< C) !e4Xh@~Yk6BUя1egB훰d1h z&wLh(^o hh7 mWrj?CC:M4ĞsG }# 24 6\:CDŽP~y_-$2$mQ`]8(xyy]h.4TRA Ae\ kJaBzc$?Ar hDA 4ؓ)ʇ2DT$gZ a0(hP4t7)40t J CCM8484?4`{(#s.O4 K2Z4 n 2t47 boXCrj#hetd hP@?4$42 ֐57848484onmoCC nȱ AeD 1ehgXE@ID`!jj@+ÐhCw֠ hpkphphrhJB0! QN&2A*4 y@ZWu ][aHаaC`u ꂗW`!m h ͡! #뛘}h:vaqZAC> n ^^^^^^=C(C *WBC#+)zޠeh \"7m =Y˫OhhC42$#hHM AAzph 9ʰֹamkH[ú vWC({~>˫h`nZaʠf4tgh ](CwР 70Pڍ6tj   e0//////@Ä5lX=fgu%ng443t Ae(24W 8 k1  };hsϦ hh]2!G 9АP 1>o"  T0gb#kphph48+xyyyyyͺ64&05'rT1te1 qzPC0,BC4217QhpkphZ^^^^^^n:ajq~h.L38Z&q>4BCLаo" DZN ֕!l :!}"3GĠ&Ad@C\7  ֡YXՔBCj 8ob-B훨 G2/////uX7?}5tg`!MieVbʠ2h`eȁk.&2arp! n V$CACZꃐX!>4։@j+C0ICW -ZCCCX ?peZ'0hÆZq? HtoM(Coqe( 84Z0sk'*@+׺rC7а!= h 8CCh(2a=Mt eH@C[ <Њ5b@[C7>qOk(u ܰ!J//////u rC&443 8@a.&Z2Ġ!GrʠZCn拹ᆪцր W//////ukȁ+lɇ3X?Wn|LCCTb¼:6BC2t eЭ J 5a0 *L2ArAnh1 5˫)4 @CC*e@!1T4ΐ  kJ! 2$!m<ֹ5@[CCCih2XhPJq@C=eM 1 7Q i?7n   ^^^^^^^^mBChC6M  GjWJl!}B@CPPj 8xaܐmȷ///////!7ĠePVhⰶ MԀq4 o hȴ)Ԕjjjj:k4壚gVSSSS|TSSSS>) }l55555iG5555+稩IS>x55555iz55fU>[8N^u;ms_p{>EQMM-@JH壚ZGl]O|TSS[6"^jj㗏ӷC[|r]_N>ms権3*G@Q?WJO5^!Zp A^/K;sjh)p)9o&!)3ncƟ@B5zq-&w1'YQHc2'jjjkF#"MaߟaU_bB/jjj]7"ҘYq$<^F(٢ߓ5cٿRBʗщ߹ZQysԺnADk}f$d<ŶЕ%`}k)!GRMV|D%;DFAD$$# ۈZ壚b6"IHD$c6>uMƉg(!\Ǹ>|-"jju#OdG[8@IF,D^T y٪ɞ|\G555,O(c-䵛#xFiKu rCRh^rW,Sb\ 9檩u)t1/LXWN_z~J]TS't!HeGB<2ذdWqgM2fiy`ϏB>?WMMf#jZmJj}2!r~d?~Si؞Ou=5Ge춄4 ">""38WX|jj&Z| lc9rZy Hw壱V-,,!´ȿ{\55TFb GBrH#" "4^|9KC빮g;_ v#"eGMqxoLB('gi74}ɲb}X"jj@GȢ.Tؔ 4ͅL {f|< % N2ʽa'2Te8R>BI~4JH9=d셷_.u D:]i̙ABfqۂpKI ;!kgv,*![|-!( 8`DY# |^(R4H_wD'iiyjjjp%crmxt!Ɣ6n4F 9Hࣣ'XlM0^*+6dY^~557<C$$TFċ:5rHbYvlcO\uG62D@$C$cuy3/;Ld5`\F %]l;va3.66~ >8G5qo=/G!)#!܈ =ƦlH#SGY+s;w~exOR>KB""BD!v {4m}!&!DEBJ۸B u{AKBd.& HQHGHV賰\9;j̵> "Ҙj}`kZ+=]S9'"2Jf_ "/|eiv *!eGM(!^]S< |TS d.6 c,D畿0uǧC5TBv9dcC/5q1 %! !i9$E$G)!i;5]}|ߞ6| 穩ٌJH"i-8+~>nyb-}X1.4LB]lijֿ]rR*6"hKDºYC{S#GT($tvx&ЏaHt3Q/M18؇B*!~;vE'uG55/DmiQZ4Md|͢r;Y[vdl0B)dK&2iOT(e3r#F/ IHƣrvf}Hٔ͆ DqTB^N.MU!\;QۈQQaE6DR Q"rS#4 3– $$H2l7qh|TS($R2@Wx_ЈH*!ڔd|bVcGCZ+2#k7\rqZr|555Q#"|dR")Fd<1*~b>T$E$ZK6 (G1ٌ>=>3vل?[]O}55ODE}mHٝ-! H\˦QH Am(XK*Ժ[~:Jna݇eϭ|D #Ulj#Rj>JDZH)!=[0RJhW+ Ѐi/'xd̏lQpW>兣Ԃ㫣{__4GU!Ñg/>/6ƬHMt݈T+("e.N|LKF, >*OGƒ@im\%SNz|5ae7f$-]cc(_0GHf^6\,2xQ3J/*ǻ~3D2 )gc8I36Xl%} ]*C _K_K*/zcj`zS,[fE?1o(E0k|dl*YIJGq ǻ{2E$坥? #y,2%\ˣ{^6o D+GG0$#Ւ;="CQ|d"WcA_bc~FdS+WS] C;~@d>"% #"Yk6հa%i,R"ҝ(M>p,<)% z` Ŏ|_O&'| D%;[jݰz(+~d,Jy[KT'^cj=GXh‘$񎬔 |DZb8#RDb:"2s`IcwG{t`|LJt 7"MI$4ښm1EvC=e1DSR6.#AIHdG0ϫצnD:&EJD6Ȗ5F8DT+|,\E*8>& AɁ+~ B)h>lcJNX8"Siv52Eb#(,M(!mSCD"(%%cZRdcE5![e36)N6zpJe`k۪eя$_LtChG>m>"+M*8ևEUG*%AH% GbRflG5%0@$mLjK>&"J*ijj=dxpp-"Nm#"Yu$JH"%"4W#~l*Et[pߩdMHE$RҘF>bƸ(%צp$ZV8nJD6jj=aHf*%LmS>ZR IeKJRDZ ?l c!=S]8)HJIT4ilC!ؒTE:c]c GqG"YƑv#2*!H#"HD6DVǜ3=+T?( H^6 DI6Ո6hSDRJVǜ?p̀H؇1F$B|<{:EbQ 9U8և|TG| )5,F$Br5w׶Dd|4rk.PSR)2}END"=ՈH,I;1V|4~=z82>"":H9~܈Hڦ-m"P68ʏ=PMny)!dBRjIC$)KX8Y81>&"YKr$$ʤ6챡L@Ć )YYGu66fH)DJ#XFΰ0P![p{сH8#Ւ{:ЈD&-.]cGq wN%IS%Y.[I۴}X*equROȕ!=nJXOGER-Y,twDJ<-T3ǘZ*Vidž#"Gz\B&~B5߾vbZ1+I));qdBR%KF0S⣱WZMv} >r1Ф*"D+e G;{GT (IkO6m68;)}7ecOt:reGn4߻!&Ȉ#-X<ςW!-#Xm*eQm,\gOLE")"#J4γp42εO)qB5XN#{-&i4$֬]RN<{j?~S/TS wO[HIٯMmXX\ ʱǴHI8KBU Ϛ9Q4A6DDQ8j5'c|\VOf@yIEa?~ug"6#X.{|tzugϞ4beG0Dd|7HV ")%% ۡsP>x4H7?c'%U$KP>""{<'"{UfpR>/soDq b$||#]DZG7 k|,jNWbպG@@i%#JD>}8>о8(Cvx̋ʴXfh;-=ߛQpm߆n"l>uy;=㷮XV'_2HʶDDX st%yF֍w( ;vB(V/]jHgH6%?[hp$C$kcŞ{CCwdPG %VgV>uS<2]cK"dHHG$QVc,2;+xnQm,}⌑KHKy  ` dDJcJ:Ơҏ7|Tl ,821О Gii>,E-C2܁T+ʕj]\L!'0ٞ"6)ʮk> 1[F~4e*AV5隶kN i鞜fPPX+2f>Ο 'luoxU> h\3>+tHp,J<1fi#zVyb58t`v>׉!|x'I x!sǴQ_6ñ+[$h;V+L}X)E4r־Da_ptLɶd@x`å[HvS>UGPߔp}|k7<[kcjpᴉ_1O򧤧Ze=\V_>:x o߈8(cdwd = Gxo\5!LEɞwb5ጝ!)^DM-w|$%y v5c>^!hIO*PB&V$Eւ3,QSf,tfp+E# H(0iGz?u]sL*GZf8ΰ8 ԣdyXei82>Gwӕkcx)8y*{WpQW<*Bkqz#'xg 9 $Ji¸wD۷Dž'%٣Qm<^12 RҸXӣ!w5k9(Y=PpT>U GK78deQ6$N JO8νe%e/H|U#UZkeexo gx/9f5z'/`mJlQ, s 1g]DM-9|j$ MTE%gS۰ ƝFgcBt#+jJx'6@x<ڪ mZi綳7g͜zu5mZ*8"Ȍvs>$(}Ztg"xI6A.[ٳ'kGǩWα2H]LBăG_8w}bdi=ٺy_rF9n[/1-ESg8Gbk5Xx|_npmZ2Џ !ET삿r]O,/!GRr%jcpD>TS cO>$OKP3J"G><5. ?1R(pG/я1ܚإ @iۃv% aޫL4&.ex YSᡙ1!y$'txJ#"%%xk Mbt2ř Ҙ^7‘}4ɻrR0c젿fcrm2>.U2Y9,$ix3h"8p$0 Pz%)%i'Df2\EEmPEDV$oM`72#uJ3$\GJcyh`;ڷw:8\%^9H""Ԍ?ȇxe6>f ^/0sC_"rA*'7*+vaiO0Sm#20xڥjLFhOp"2WsZBAp C”TZY(?9 c6tY;V61t'Տ;H##9Iz ٞ6K҈M}MpDꅄQyT72m80D$JHABE?`\ DVRJ2qXD2R8 P2UG=vnE5L\eH G ' "kmܫ';R4E1wF:sR>sloeDD'MO$ DtmR%%O # l;C>kŽ9y8Ã69 zrn{M!pt01r| ST2zk[qJRGyW[E5yz%t#}o{3p $d&iQk DZCwn6'Fn5KE1PgQTdG4JIL#>>3dGO(/J8Umo! Vd\ZvdD[k<GG)bbS4~sĶKl8=ӕ- iIqXOj2 S2$#&Iylx{b)l&W`t2ۿ(cR<~kR҈H,K h!㏖YkxۧKl%[_sj`(')()-\>?#0**eyWe$R95IrrY[?".ƒr[=[< H kU`wo)G;^6MX1wAciS/ P{rXtVv`rPNra MZJm19+9?XC-ߌׇ*?f1X5(dT ֏4 YDI^\n4 `)xہhTQJQ 39ơ^M)8\K Jxp GfGsf#öI<rI |A?>^TrE,,-[ͱ׽#LBJ>l_Z?^5@I [B"R(A,(o)'mbOzRɣ $=Ѧ%Y[9X%sz}ʵ斡&~ F>6JHDdP?]=H>⻼((/JRכVI|$ə[UN6Ր' i,*1mQ8i(0$$C?7<>ugc2(!H\I5~$kv0!޾u F-eqꄌՔ I$Y_\G?nxt8D9Y!'â贋^ubQLf92.I=nIIJ4pY^̾19pZ2 $Mj'c6=p,i&>@1{"I()<):8hY'ёOu$^~orÃ:e"liItTS:sdsN3 Ad6>ȣC 鐓T629itD`"V%MR>G`bpK( bwNkca]44L@;G,{4ffN %Čs+d7|L4*yw c $$HZ: Mvd#vqMK䝃1 Re#9qH0i lhLH!4Q&VGy+H>" | -OK-+H\!5R" X2jOIْ()kw*&ѭ.?…"ߦvpf,&B;3x`RҖr8ԏLB |ddr2*dJFLiTՉ;/!dikNG]Az{{"=$Տikw d) >ϓőDl3$mn17}(AE"" GlsO4eD9)倌TdOJ\F΄e ߕR-ΟLa@m (!%D<汌a΢ZNx¶BZk:+ Y @$c\H; nӞrF2:drrj4ٞ,=ݕmR-NSY:xd ҈ mz&lc-)obX" #"N?rX4/:ɚkDVu gRN&d%'|VS_$+4FFFi )h7dJkBH\4>׃Ou3rs8eZQ|xEIV/vJIv-xq:( m1glʱuЧY.ۦ">"?5("?oACuFāzœ<)IŎ|hq*J}RwI@9H$lO̓W>PK>FјD iQ8JH] ͍NLɁ: w2#Pm_'m2L0w$9k1 -OpG:&\0}h#AL=!k5nDg!BB|sk놙* rU`f$&+8­ QA1tjrrtt= JF qbyS͇O[8cGHZ.n4έ냣i9GX,H\]pKԭ}LLK)EN{~Ƭwi1S\Yj8(iج<1GJFLS>b_ܖ $5Qs`$1 lŇggZ2'^7)OB '̲C.SN769(詃C]_6>eH+cDjS;_B4>?#l@RڝB0wdJoOv&'ΤeM@74#q!Wò.z.E?6͊cZJg8\_>67J7&V>[‏HGw{(Jƛ|:[;% 3dLd:5׉*]c5F3`rWb]2r &'=cjUeJ֗|~pGBo#"%"#>U96-1]$Lx1?33ϘG7v.?@$;O7Nf\ۓ3Զq$#+rlc_%=g+‹s..~?"1o|DiEsL.M{wwuPAHqzb4lL(4#9oFNJ/YדY1Gdec&OPΉOc/,pl2(Lti+{2,N&$GII@MN]{:VnjW޲|F>p?PB i0E&vdZ.'MBҸzc*Jﻆuù㊧sSN~bQO)'{¡6 Q1Ҍ£yhf?E$4 n& $QE" &fӏX.M >hwfS[g9onEw WnW 9QtUNߡ.ʹ>.fy r#=́HLg% W_ ;ac# Yv8\1ߒg\zI&'>a1;C.a3Cm`e\66_xi[C5gb̹>Pؕ#j메LWĿ6),ǯ`ı*|*'S'$]9ಇs31n>A0R},dD )[kzy >$)Jຒ1P 7?>wWzGE8_3N]>18_Fҋn\t eKĜrW&cƀQ6FʱG#%aͼlVlLLo۫)"}r2G[O*QF)Yx1O~wrrE\NJU g{b}2\H{p(xcx܈g.| up,azC,XqSQ(dwk7e#{('$p1YMqH{\gv/x qp{;WSLTDpHH6CkRM5:.uBf ѭ݌$񳔒gL>%{rcBoAd@ _MVU0m␍d/ou0cgvHN1Ʌ9 B&'(d-mpU>ʀC32DDSɸx|s_hco7)~1_L,v!1}dcs]kC+-rrmiy68pYxle>D9c@s&=0]ՠ2rkb(-lIMz6܈*}jOh\ӷC aAoX=vns<E1?mzl7AIF䣬qEh>~_Cp<)`__Ku`=7N,#9Sz|ֵ["\^=[B('19yw\N%6"],>y^(%7.1h#.lL`L0RrV&z<ؽF!pʃ%ďUB]2߼aG2^u"]:(羈= 2!)~c($jNhd(#hѝEYnKќ ጺfS_`G . %7 m֞MXP2ɪ&wR{O7jî|ѭé.Td S<9ȱGx:̦"D8Oɨ:YLS[ϟqp3ש4\& fG;iE~n5ܔO^$~0cE,B9Gt^ru?tzMf:wH2k5ms,s=Kؙ1͍.ʓ" Qmhi# RzWYw;v*b{(񉸔|4*ε :m6J[rUYD1oh9@WHy1Gc6`f8] 3FdZx("ZGM1|-R-6=> 7M+sTCH(ȆКIFcOfTR?q#&R>: cNyyzK9:x0K&Ch<4olll0.b1GάmXpg_2fʱywkeן=qqOF~"-ؔӨ=Q5Lq9Q" 9uΜTWANN#i{>Z@IY>:7ڠvة˃cȔmLpcDz.-/*7 k\MTpNƸϚ)B99wlD)'zr9UԸl8u2G1ݾ5_!#߃#mp(G`(޶~2J-0_2MYH4ۜ}kb*&;GbmKUJaThL3VPFd8c(PH;t(M$& TD#]T낅E%$i>EN]aO<ԥ3WToFڜ9q (dO|l9|LLPnJjӘT`z G:*xLk)e+iWduQ$zH]8&VN(w2`@9 z`fPMY2=pSr!5 7c#i˔ @8ۿ^xr8?I'0V$+fsaU/Ah(~ 1cLk?9}eto{_yRDz*,9/Vf,{#uh-W\Q92D2\c5v^㿽TܹPN:5ە@nhsϬ\dUG DŽLǡ2u uO[ua_:כ7д(C_јp5(sS]JNOSxkL@FOm+P|ta/g c E&qo{y${79D [Iy9ʗGF>fby,pWMh?Gc݈Ţm.\4LL68vEy`THv<:(=/~˭=N4(UW{m0]R&@'xVucOaԏ X=?dӏr{ѱvh4DPJ ?QW?y7ZD Ygu\8=0:H;d|Y2sRF~2c'g==SӃ:qW{T#$| c瘙:@4]2s>v35,MlF1q³>xd\38RC|k{h;]0-FFl0X$FĢˊ+9۩'6{**=.FFh,"IT?cQFp'Ggքۜ_[]7 ?݆E*՝ 8/+ [Ⱥbj,u|t}-~ ;7 =>J&Fp$5GZn]I `X;-S34 NpW!eaƸLV9!Q?LJS s1UH(tp_OomoKyoٞdtlASi2q,%bd<Ǖq8H^ָxec=S~D _G3͈G;ƆxKpLI:6g,8=-,󃾓&nbHF_x |b"iL_pd8V:-ro0 ȺVbgcGֲGSrbbyO d3-S[>d|*w D"l:!KQh,E9{/{іюוWgca|lXvrblL|-6On-Q9ֳ<mόMTL4JH#p4uTlu/1ۛ) bE*nBa 8S1u5sŭf3"2t>|,P*ڞɿn#;;5~iG\Wch:1Oh_;#㥎ᎬqHᘸX*k)d4Ά?Tx̃Z1lǚDz9R:>qFu>~pȇ#]?/ BdQjh|ћ_GIȕ3O ǞcQ,jbGH\y<әwKp,gM /,k|"DcLt׎\{z؁w[x8@X8!O8!p,V2v ̹H؈y8d!趎qŠOEdZuE*߉E>")}x̘ex=5cÇ66pl9+ 4ŮOq:NFtVkϚXs}xzߤ~ɫg济sc#;]u5,5KŔB!-) J>~-3Cs&ʩT?sp\[6D6!kE⏲Gy:ٝahu}sn&) L77p9&Bd$`}XsYȥ\L+[bxKFQ08u֟=QF]ոp hwG=hO^GO!X1#E$=fQIsbyʳ!}p{cRiy;+ӃE(Tk6Q%tQ%}>|̣cdplq͖i$MyȩvlpD2l}+l)@AW(УՆ!bqopRr C=h;dSY%"}u8*< X|  Fd/gx ^PjXWqE|.;d{?fx$Ohuc X c|eYX #D~o#T2 C,GpYό;QǏ3'QGs`1aAXF E8FC8Ge[׏,[ZewYgj[Пw[y|*+LDWMG >҆EwfMAz--3cOD:j0qY+ɫ1"YBfGtdbRo _7$e,pt^{ӕaJ9)IHc0α>4$;=rѡ*棃݂cyd̙vEzQ<5m Ld;<48\?)!QmC`{~{[DƗ 2D0Ǿs?whT R[ 8VC6oUfcPӜ9[m!cQ<>:_J? x~|EaG**pc-3/g:SN$'t䣃c3"v6L\5]*"}p,U`pUGc#qDlu;dOx|a:}*tD'.Ha-wX&Eʎ# ,}kMyTzphMqp%=,bmIc"ˆ%w]2 "Kk>QTQ`7Y# ց[ 5 P><:CţqeI~N?j!~b&{cb&+cŲX8Z=#qu|"7]4±aWfH^mF>Bym TB>1e$[ 2D2i33lޖU?dhG m0Zu܂ow۟r6e0i-ij^C2 8:t=bGۤsb6 P*A= lL->&‘yK5VAI?1'#1m1\Ĵ .{EnwA[)|};h2VAi|0IaCjv#E YyP`T!82h:Nz*GϺlԔG&v6)8+HE6 #pٿ~x -:8ނef|]fpoQ.W1mM`3as/F ]?j=/vOG<)L _ Xb*J> 8rs4L2.xa.8绲"ܴwiS& ʒoCeqJ}[zaR'EaǶ(5W[ݨp)o{%!}IcZFff>!=ϱԏͶ(l|0ˁ1P6,5&7 |vR?h}@?ׯ["!*`-{>|ltK<Į$3iZD\:٠2E_<ԏHD@X\| Z|,$v,-YC(;tt9va+DE[۳o‡-ddo̗dd> }Y?vjN ̨}k)fFw yre]Ipn}WNl >'Ʈ5tȔ;!e HBԏ/3,-=A7@]xU\[r+ȮѷnŲ棧yQ"e#?~Ԇl.#X o+PBNJɨp%> ]i'y"ɏ%mspQ?x"l?"ÎX D^V>MFGyԚa>;}gv1?WGC&*|,Փlc|4r6x|]m#S薁Xg1Ŀx$_K a%| oofW >qOCuL ء| ־;pLm n o:Xe|?Kƌ[85}겍cQ;5T?JjzXz`'Bl ЗVQ6̘rJb,bw>NK3 Q?@dñb2&«<8obkL_3PQ8}p7tB n[!޶~<%8؃7?&iO[9wY!E# !wp2>\6&?&ñTlVL<63cf z"W Տ1ٿƘdcPc dT>BGO,MTcDdC`3>#ŢkXp`~n P .l8c|BRES D͹6ۊ KփxbZBDed23(Ƿ[}bܯHHJ~{1#wpy>DIHG?Q n1-yp>،ikyt;T9Ҟ@1FwB8>Υ|knNMGWi(zv1POcKF8bw13vO{_C~E"e1R,0A%@x kξl|.kư5_z3TAd=!d#$m @%V+#]ѼvĞsG&ecx{T[1fid}Ȱ?ǚl?BJIյe#\љUAk6̵uq:?em:͹^{9ngxȵ?fH4?=xno3 #ܹ ̽%mm8npk2KFOf)h(0Fa=3f1ioƃ AΡFH5&!i[P䌱3,-GapaD@ s]'OQ`k8v$eƲG,i#K>km O%sԁ)fQ(yT#è%fi6Icxμ @D !"q֞Љ]p fZ4v%p|\&wpG@bw>D0!$\fl1 L}|Dtn|dZ4Qət0=qz`q*h]XRe\b ǚj1p1CS]G\<28 m@ O&3iC1*GfXzڿZ*G>xD"kfXdYfǚ?8.A';na8>֓U}^n\1 nj #f||D|C(pᧀM:L,aģ3Z58egS{Ih1Xp" ^vH3 skXd,DB6lp Z:" ŃF8 4nR_(hI}vvqg|[L%'p`S2AHlIm/osKHkEƚ1u#U8o}?GVhԏ9L#ɍhGuMǬ#S|[l%^v, yt#=v^a<w1L,DBX2g*6nXS>(G_Hgľm-vkЛgkYRe9bY!»MN- *}vG*O}PV4wf0fDϻzG'^m]C_n ,Q!N} ^? Ǻ*R#|DD\jd ©?n,6@hڻ8v K\\#M^-v gyrRyՐUV8&q1Ѝ l4GGH5IH9VCzO~.68ℌ6HNF Yl`kXP8ё*2Xn>ixͦ>ˍ}rFƒ޳ B@t55ڍ9m./9jcI9P٪+!;-.q ž83/v_ -!dH3_`8Jf&ޜ4RD•HvpɄ|d4F)!iM{݋bxa>5(!m+Ɗ32;|ތ'g! 8YԉzCt|܉npT efG\Iҳʤx?6+%M/9DW1c!ᔌD OaҰ2=Rm(_ јHA2|oǰ[YmƄ/',W3bugqL pAeg=\l6)_RS l(L+G.voBS_>/0I˾71-(bpG/H͠D_/8~0|K뎅G AύVMumO,_X1}cLHR>iqxǟ>t/`}ژ(@*Lp.6;I9qL?rh>^fr`(Q-J߽hdcF{\Q8H)}g~GA<InmöG:'8B!G%'?!uH >hd" N Bq G?lKWLNtQF%{Uջy!εQ<lŵмy4zĭ\ϟJ.Tt8GגX(Yˎ {vd݂hD#ϙ=IRXW=2n\I>BpĚGȒ-d Hžj}(K[ s;ڶFJ↸і2S."~|}PVd{(%áeò؁2ڲ.h\tUO ˿[<}"8#x{3,#{m1ɳrT(X1S $CCBj#IHϺE7n6?q}ډ (8:^beqڱm>vІBGܯ-!D/4G>=]N c&, tΙ3>{~$7ro3nOr8/'I^#b~e[:>D{fT>R::֟,9~Ze5'-˶!Q($ AHH ѱ[?o^m/_Ndcl9JFc]$Ό4-x[`˅a0.E9E_BP~:~<>_jg"W?_sI%^7m"˽߯X's?JVPY`$TU4f/Y;}Q>G_-Owx{nuf0gݛ˧<ĢHԏ6 $1 G5sb`>Eƕ_ԅ9F k]]%ơWcGOK[#7Dex$< ȩÎ0%WzD6Ox ؖˏXO:ϒ{FD0~O`0sk|^]Ÿ 1٣զDnd _fXyd;\/"o;%o4$'^uœԨ% ~d!H@$(,QG\wV2"SE7mkW w6Enj}3w|-KRdN9ۆ*HSf\۲4Z@'_SHh|@d(:hDV2) \P[ e2My(Ǎrԝjhc&HH~sOR2){k:U>N*ҶaK.6qSPGA!S%s5'B3HXnḜA֫I8()5ҿNpIY4^NEd^[?zt#%jΘ=b?~S GK~ujibsXUۻNt"JȄZGN:jt*F=Jl^vN i,i7s,jzJ׏pJJx5J3>.6c.#CxD2mx&!Td|̀HRz֩4ۅubWށ7Y\x:9X{IJ)}uldC 5-ƣf8k@%'"K[?RAC6|k۰5gM(&iǎѾ7W"?-)PFZ%Zm&!oQذ棧{wCFz-(RĶrMqv(}ߘ`/xbo+'{be'L?@cqPҽ@ERDNW""=畚q 2N,A7pnyu' 8Tzb|ZL^ዾwo);|T")<7|K3J-ŨAh\w\{ZRfHBgXGU v|oC.M1B _pm%|TS>k>^b䣏+?X\^xUjG#_x}qyXhMG?a[w f>^v;Bg;6ΚjGanBဵvKKvs ixڵIG5{k~Ʀ>om~k[2 ?7Qg>u<>;aqqGmo_tZϨUw1.X^Vy L?j Pw8hH`F|?__#Pr7gZQ?$ͶTĭeӮ?w(>Ix` }?Ir&s49M3J-WWOr䯟a4;7 NiH` h~3 'bԏ (Ѳ\~ҁ#1GCxȿik}):?OO(ϼG4壚QhM":{ ? V :hoOP>y1.\^VH}(y{gGY؏E#qZAXzg-ɖ1>:zFe# >8܃|&ѹ= A̾X xG51cڨ b$?C}|ԓJ-ŨA/і~:gB>C/MX33g H뇠R)պǕjeJM毩~ kƾ#S@_+0ȧ.I#㜯'Zq jD4V#̿6 rĿ1ЃLp;?|tgj CԳ~d3J-ŨAgϡ,p aqi 墽:8| (Քj^|qakm49!?ˑ1h>6Z(5XB35#k[ $ FT[ fgG0G|Tҏ[Ο3ז1{yCag 盥jE^WnP{WO]!?<*{}_=3rgZQ?1q2# |($^6hB@<~y\?*Քjx|\4% /8^טS(J j 1*<06?CU$}o1>z ғr\bԏc#-W7r|' QO*5棣ǑG|br:Mpb#rqBZ/%6ʲ4(a࣬70, %>_p>~d|tQ(~ǣ(w.4lޓ8L Xx}c,Ϯ|TS>qc(2܈fׁxc|$&Џ?/T>)up #r5H3U?ՁjeS㵿m~O"?1&0j!&!|Df⣞TjY/ƥ7ʼ_1a\)>Qr&fYo ǔ|{Qj/Fzl>=7cn>}D'|TS>eԏ^Z?fqևp$iQ|kM|vA,xxzOc*>b ԉ^;6(ՔYx8ԏ.><~lN n4h >*Ք8s&g#k42Qm\Vo +o5&eǝBy=iH{h##?f89mJ"|Cc<>K|l=H ן=QVl ˿L*>п?BGcGL^Wqs1*{#ӏȾNLG01G>\|T8G3hQԏT9l|dj]Xg y1O,iF|D>SRؑ~?B'PH>BHL號Q>)Ւ[

uoG棡ߕR6.k7? ᄏ3p מ|d)lXCmQMV w^bfn~>5Uglεc~_A|T+@?#d>fG5Z >n B:?kGccG^+n]m[ɿx#)}#J[؉]4r7% |gZ1W@_#[GY G<:KcE|Gq 棣 }mxc|D*ժMJMiQr##EVAƚgLYQѸ #?,R+b-OQ~d,RزmE*}3eqNR>}>[ &fx\#>'gjWTC .:o擭H?_*-t CĻG)VŨ|>;O\ckk)n\d&gjGcqeQ}=ͻv29C0qmG5k:s!G_(1wwV-?rP(Քqq{p1h#ÙLI7^a#݈T>u1^n=`\aorxʖx<$1G8z#&6K-1؈͜noZc=h<|ĵ>c#>G8G!jqsݬD8l -hl|)(qjܵ>\D;7eG| 3EX Ƀr@$T5?l ^t+Ք]?5a+hCzR?G| >~y{[krIqf|9>(Sq>]lqGe"W~1YZp;&kbϤ*0 {螏U*lGX?]ѩRze3w[?j=d.>v jb?(C}Ϸ&x?GQ%nqf|BdžeW[̛@ĚOT>Gzw/k?!3is%GHh_3>%ߞ|T+63k]ECA %|)_#8b2$$Sy "@F| "9L=O{bu]?2eB66"7HL?Y cgs C)Jg *- c߃ۀˑ60^x~&Ԙm->g||Ӵ,lIiI]SH|R?,\S8|,W8G"|}8>"^QH{yӁC5d\ e{;YyPUU!$̧`kG(0[0l߈g~x_xGH#G3G pުRpe}X}3r/6"8ҁ|sFO\!/=- ӏg'E-KADؒ3ptxY"ojQ|l4㐏]fxD\19XX럡|a2Oo&3H_o4'_ QZ|0~dcHv*~i$y>3mbۿ +KȜu4ȯuB`zݼ*9RgQ Loӏ؍'͆BH KDB2;o x|^Qm->X/X,x1) dZc.cH63(Wb~0%ZFx_Z>='SQKwzQ 2(_{f_S[) j!ؤ+1ȉvSh ju#Cemhb‘"ұ^]3Ȳ0L],h߈u=?h.4=| :H/]JiKZ Qhlc#[JS0 9X~F2?'S+O9_BH PC{3"qYl#LVxnazߴVXJ d6@!;&zq\B@Qm13%Qɘgq0/;1?LTe-P#U"c5XWPeq-d>&=&$cسZo]]{$Jd37sSS>KIjjjqk׭C->qZhڴykM (}h1>oUMMm*ԔQMMM|TSSS>n٪&yV55555iG5555壚QMMMM|TSSS[lUSSSSزu4壚QMMM- o-srIENDB`PK !p:ppt/media/image13.pngPNG  IHDRxD pHYs  ~IDATxwlWu'+7tb~3Nzgll I&`2 IȈ$ H%$%@$`L0` &8q{u^>S]~Z( BP:C 7?: o^Zwޥu=n]w>k~O.}![u꿚wǬx:DC;jA'tݧ,- k~3h5]z7\?%=/P( B &v+CFk4Uba tx -l >hvȂBlBP( h4;h+C5 ]*{.Z2qBڬ>!/=/P( B No5!/3 :)9J Nk"@ז }|A)BP(Ѐo_qxkQ[!5OCCZ!:bYCC^h:ЀBP( h0&4 NeP֐ }YgoE5t"5 8 1 BP(Ѐ]}102@Ce ʐ%ڐs4bCCq( -xلBP( h8km dd C@CvnGVkC ֐R1 4ЍP( B M|P2YC c 5(Cd! O12[᳆,FipFZFg.3vDv8?4t';;1w7uޱ-Bٯ$ef|%C+t5Cd|ۭNSQgcC˜]-ҷ# ;߁N/?{jޓN}Y$B^Bb/FW< b hHJ8!!GB@Wq^]ghӝ_3#_KaeaC,.v$;`f7绕N_Em?ܴz -fh(D.f c}Ց7X84CX:]Xy"Y"9o%F" %CѾpf/}X= pw*5}`;}w|ڮG~9iӏv&$Yzbg9# _NWzt^Up\R—_Vƅ/`N|VQ%_3~8OXy:'|P Р:,ϒ(;{7H>аhdp~( "A~"Kq ,/8?D|2`uCqBKn|B^y+>, *::]lWEM.ahl'<扮01/[4hhP@΍'큍&w6[K{lϼ-?1~q?/<5.~'59ճu";8EhlcW> :M||Eiف}h8 NJ%Z "2&5 A~(u:b.?F,g(#p&Vz?a 컁~ {S; ]gwa{15A'ꀃ/ ύiYع wt4/0,4< GwY44 XЏ9x4dy`w".bG!gOLsՃmcYD! +@;}>}KAclkV%0Ȟ`1z$hp= SDBC̉2Px/! pZE ,~yknhP|K/ JCzXCH_' ߌ 'N-!ߡRCodK`@4!p:(:=?v.3-滓K4k >> Аpij#|&MS!4dfS?l:gH@D T<+dg>􃆰]&|Wk ah \`PwcXd*&06HhP'lO24htHh6qD8dVYha5  ]ks!֝_l.hHwf|d^gW+!<Aa( ]9Oo@'?Ӿ鑱m;X94t=  @C̭;\;:оa&N_贛;>wqrC^h\y 3yHhPV @6!2:!³ǽGcj ahP> j@e6AN?]2SxPJ"U"8 rCxCFej R& } 0w02%P( ЀqkQ *!;d O  tۿK_ѽˮNH 8dTbܠ!;4$XC׀C5dWEր< BP(G-qJ!4$4$֐f ZР< 2*v~4  l rD\044񻆋6I y!$*Q@ BP(@ߠq!răoc @ W^h+trt vKeKce֐K@0hȆv7k nHȖkЀBP( h7(VƵV 1i !-Ta PRTAB$7d>p >T ۛVe BP(@@Cq8A4lACdr!g`hq2!l JFAF#]áCFnP` =!phM=ԣ  BP@;akŚo8he } ۲m@vA] 0b́s! Nkpb C h@P( h,ִА:EfhCn!gj AB0b! Nkpe NqG ( B |kR5Vm A֐mhq4k(#5d ;qC2!5 Tn&cʰyýqNyԉ>VZKtJ}4˷w7uޱOW23yN2l N7W> }U}1=PUิӭϊ @^f6; CWoySܿ(zOg l8i 䮓]Evu@qE V:|_+ iayW j@ LJ jY.\Hr3|+rޜV:}ACsV -fh(D.f bƾ#=1yhJ"|W "nHnޱ,>iw_Rv]͒' AB2QB_9© NeHVk@ xn;C 1mybL iР|aDžsƠ!˷^|xG>>?P0lP昿r=_u1h` BC9cАa _O24D*Ҡ!  : kFh3(C$4Hk%{}O]!vj  1`XCeACL*A30k y:||)B3tyӾŒOߣ|`;}w|ڮvu[y~oq'^ǰ7wBڗ{9;{t^UQEo$S_Hޙd~Al|9=IK>?KdkFp~Q꣺A8=S} B]P5PлL |IYZzW^쪰WkY / AvP 1]{1 a *g: *\C| "UȆHh]0_K\>&="eSPjHh`0p BﳜlaS >z1D`"C N+b ϴ_u K LK&|:wZCk,48OwLS%ù&Opp8|A|R] D ")hH~j= 9WCO,lNg{ڱD3Gx?{ `WIH>['NߦG)BCk煆q=i w)!Avb\O9#Xe{B"o*d4:Up.։T c6MXePY_~!pZ]U [-,:8 |NB7I 9UBK18!`7Aw*ZB2"3 ۳ )FN\ ; g{.h+"4T»N*43BCZF $LhpwI䀆!l+D48;&3+C4 MI 8Ck!f˘a]hP&F(ҠKBxhpN4v gOa1༌I&{eKBC̫@K\葎|dZ,w䇆N#xhw vpath"u}|K<#c'@@=,z~*N+Y6Ƞ4D3 |DbDC4ӑv:}9߇?!w]S 2 oP bf4s猆76hp~- ? R`+ubp[hHx] 3 WGQ?sbFhH0>%12e?ijN &0Ќa։P'Gf:N (Nȝ#xh}{٬@Go;2 dG'4(q,C@bR hp^@bvL։041 C80t ! ]B/['9h[UӎG£@C'@~^hIhH.j3g4Ap-=bP;8{zЛG-$GNO}FCc fdA*g4 e y1M̂2 =&ZG3DΆgD֘R}nDl"APtoET 2P7oVu~ ޲{gjOh8bKaO41P`]_g!"s0g]ףS?{_>vHvifAXMޞeV2qӂƑ# {BC'Ww2KUd܌YNR򄑷cb;P!KnP 4DhC.8+4bh kٽ=)‰I iߪ3tV=iNMyS;t?0[]'|=! i qV-Ne`h 2ߝ-vݞ|sZ{Z?+ױ::>+aV\w'|'F̋ixoSlKŴ'Џke%<: );KS(,z~*q J!)wBC!?[$bNއ (]N9]ߓP<3h3(Vhʠ "rF>0B׬t6i"\@sDxk0507||02&@ *C ̃ B Uqd5Cq*ChP`!F@rE5 8C23b A~8q֐p"Гlh@P( h&*CmʐpׁVhn |MNhK*7rCx,\2?x#պBP(Ѐ58CkBҠKDrCX"u rQm8CP7tD|c Zg1H_h@P( h2`M>P!4t)֐kBGӧ jn1ssI bcЀBP( ha8e C27䂆֠S q!V"!rC:P5DT$XU1|R h@P( h4`HD5 1֐"): QͅBR uZCWnц7ZA)¾@P2P( B 3 C+C+4d2:zFl&rCcCv 8DZTK 1 eBP(ЀU/ 0"4_=bhk: =!Mj:!Ү6|$!ۿk,4P(TPu _{sJ>h4 P  (xI 8hC24$Qtj!+eOn߷>%9| ^$1 >iw>$OP(jb0w] \ 3Q x#E}ҽ+;@ԁ>АmML4P,C\Ik-N|!~$R}R @B AF\ gz d<4Si 407(kp!憱! 38 Z/gCڕdp֠~"|غR22(kp!nQ( 5;i;۫ q>yݐt"{ b[ @ 孡<4g G@C|NP34\k)M|Xn0B Dhιw b' lh=Acbth,@ B44#kȎ,;ȖNFhA/! j* pCW I8T 18/,? |Ut- ~._{Б16!)At+./7ЀBPq CI- y $ XP!ae6jF4 Q$4FABVeAZe5'lWI s ,D}()֠.<'(!w  B CC h4:48! 4eA݁pàm>eW l !R bNw%N AFh9)!|цAJ) J ( |zijaEA} !@ B 44rhPG}xBQnFhKߠF× ܷ4g B+45y 07XhpFC80]eCsKf4M91N.l! 6C "2K:hCrЀBP(*p8CSqM @C^npB/ڐ&34/@~7=|KZ>M=PBP(h([)5d憱!~CZatk`nVkMVeJ P5P( B s *ƛS~Tm (rAChCmȦ  oŽXÈ )rY/nЇ ( B 8ƚ8COPCCakПm/Ъ  t0VEBAl Пz*øА BP(@*|GCB= 2PI!AC H |zܐQF>'oS8H a\k4P( 4H )$(CIkPc #?;$pC5t8& mu6DZ 8m/2$\04  BP(@*  eAA~d ?:tHhH6tRh47ѡ7ķQ؀CC.bȨ cYBP(0q7U2 ak`hqE4wN4Xe`hh/d5L&g 7 ArC.bhhn[P( B 1ahk an(q2Z! ܐ` *Cakuk& BP@l7M|Q5XC%j Nh`ntnHhpDZar0!kt+ܐFP@q@ BP};[@ìni05aPkP@w;,l h6 ;5! qHh7g !k!{g4P( 5|1_& 7hk io+m hixk7Lo"5tY4hpjhgHšrkyh04[]L]8iЀBP(*Slp^ h8ÈЪ C@S$4587 ;p!6 2  S 8?Z!~dCh`e̔F<^^L1 q:BP(zHL1'0P-4dhi dwu2*Ck\`/cA]'F8?I7|uFC+48BPs<:], @È y!5$@ChCrYCX5 }' anHh39>\, ( BH4 Ѐdz2xe >eAQ5Aà\[W&3􇆚Bkj-7ĴQZ@ +82`F BP#@!a7M@aOBC.kC$7$C@@p#Nege urC-& an( >_[ABPѠa։ h4y5T 'wWk>'Xs 0PDvk2g[C<4La։{Ogi  Bfh4g(f ]!5!CC9T!A,4zBnH;(48 65G}-!"g4P( 524$h8c׉+1)ÜCCdN UАQk( anH -'B= ,q@ BP! u%}g+1Vʐ C[C4xhXCsc~Dt NhEZAAR\0507. '7tq KeWhpz Z  BPcً3 m `$7)ic#3X AC=qah`!I  ( B 3T ֐ >k`h`n`kP␀]!  atnF vd*Q3P( B 30fQᬡ'4YC48AA6DNцVh6DP冮3 P2He4P( 4=05WhhU4Xk OD 4DZ Ӆqak.>kM ( B <%Nhj =!}Сظk+g(l cqCك6pEА` e4P( 4fMrAC^k(4(k@CL+:4 X(Pj 5+CWhPq@ BP(@q֐K:YC2@04qC+:8 |3T0 7*4XnSZC8BP(Ѐ85wR!mHgo04e 5Ԭ Hhh@P( h4@Ʊi y! >Рa,e C04g[.:dgM ( B )=tʇ֐]b!R:AYC<4dU9Zh|l7N[qVhH3P( B 3 @CPCN:憱k( 10K@OUYCx$BP(Ѐ8C!k04F0.4c CspP4(bHqahH3P( B SU!S4+CZ׈\ɣf5'qP0f@05T Nbʐf  h@P( h4 0yhYCvhࣚCk`h k nHBkZQ`#P5 @ BP(@qѬa ^/Xkx/ Y`khxn0~07dq!@ Nhk CЀBP( hAQZആN@ _:VWq T!>P4Tk CpC O gЀBP( h@a!ц4qH hC804Td07䲆Q!PAAC2P( B PP54epF8 mA|P4o Y!`BC<1 ]݁A)CBЀBP( hX(c ŔAZCWe`h_<< 78!>ЉlACU[CƀCk( ] [Cz ( B (ÜBm]!6Hq4 >)arp}% ( B @PҠ]C~xOnYC8XC+4DFjYCnc BC21@C֐%h@P( h4@2XCye  6ĈCk!"6y>-k N/q^ЀBP( e \C! 2 !{U hnH!g!h@P( h4 ΐ |\|N'q@6$" aqpZC'hOpC5䅆,0uhM BP@ cZCFe˂,7ġl!-6Y樞k 5q,`$;]f q@ BP(@0hVk`nzbmH8$X 6dN1'o Y!/1L ( B 4M BCBWh !|цNmNqP5BOT'X@01$@CU֐=h@P( h4 b!,DB6(q *}AAQT |š09}kH**Nq@ BP(@aFNh`kpBp Nn Nk%}AFjyA rAàĐ Xq@ BP(@ag@PLBC^nD"[*GGdI7pkT041&F  BP@ EA!Pd! Z)n[C8m e.$sCАԇ07&A05tm7 BP( h504tډǷpq+4~?wX:qCL!pj R5^?EWq|wnYC 4#CPq@ BP(@a6ᐎ峃Nkzx`*7i Е8[U7C ]?::ACab=Gk(0h( B ڡ$$(S!625zHn*ZCW5q}pr2'CC8BP(0SP^a,kNNh3Ve@DYC8 7==,{:'(Đ [ph@P( h2Ԯ BR!-:):C'kPg!R5UƲۭrF1"1ܞoKBP(P㖖 Z[CޛSܠ!kA5tRe hC889kH[rDb %!A ( B ى3|P-g!& J*ZAC0I95 *BP(P1 6k( @5Ҭ! 5sBʡaV!Rj T2q@ BP(@a0-h(i CPk!pHx/+Iašfe+kpn4S@ BP(@aơ>ꄆzUJZC[nH_8V6 z4̛5Kfo53P( B 3Te PJF'b:)|hTe =gC&D XCk`hAC8BP(PшQ@hW2P~ Vn 4g 05y 5Xp>5 ( B mBP7XmYO5$pC`VАg,4k ӂq@ BP(@aJ05$@à0 4xePЕpe ٯy cY7ҔЀBP( h!Mf$7[NР7!bV cYô|ЀBP( hXaDkHathH6؀C2 p'ńaV! 孡岆de4P( 4@gvG!qU5hqCkqmN7h(i enЀBP( Ѐl<5!hwR(Sx>\hVnPnG2@3JJZCIј2P( B C= [Cm`wBZC<7t6L f⡡5 Ɉh@K}£BP@lJZC.en BsC 1(nҸ!l Nneaf2 F3P ~TB ?4АlYChC+7Z%a!iAXq@j@ΑP5 ^B 4M !2dچA>` g an憞LPXfҠa k(f5g4 9}Uuž _P(ЀP YFh]7! 1 }`h5$+@0-hH"/ԔAY;024'$B j!5 Cn k䆰5!`,h50 4$XCe4fJ5(F<[$epZ t@P@ZhNXÈЕAZC 7C*nH2Qh !  ]4h@$7C r-4B 05 =a,h󥴆HnmAô!46 2dA& <OL k@P@ XCei 塡O7XkhVk05Ԡ Ӳ&  ([_YT!~6deO{" ( hW PHk 1֠ka֐]zZøo e LAА6r70:lԟp 4@ XCIhHbА=Ȑ7@ܐbjZk( hYC @j&$:!4C Ѐ( hhi"5 a :qܐkh5T YCed 2q@jvZkz53rͧ 5aOP(@P5 !TE  CD5ԯ YCIhi  /y˷(g >}-i Ǵ~@ ~ЀB ʿNhj 2t 0ih CC5LUPڃo:847} '5pf6okh޾{U) \gck hpZL+@P@ ZC=i =`ANj |NpE5LFg +&M1AY@GD RN7(ep4JxEF~9to7 >z޲ja ( h2C+7DZssdCx$"fƵi Bݷj Ԏof589524ABÎNKlH4z+| Yh ( he⭡6e4,:1!T! ,7/LTFqZCP:]!M Ǖa@WCNm5 hP죥G-z&Dwn}\9 1󌆏Q3րB YCd0Anj 2 @LFAZàpHʆԠʠa5`hi{504Fp 5PCVj e`nʠA}h( e2b BC`V-ExH _rXAZC.=W}@![NK4d-h`kPgy=hoK.m@@Sw.~#2ր ( hզxhYPx96DZC МxvȄ5LeJa8ʰѠ" zqлB 5\CX S Yo?u/AWMjxNjH485 s -p 4 LЄI(C5 Nk9?(s5+! Dš57AEG@j g".fg&Om k49YkjFIKr09AETk_:MW* l 6 (x*dɑthY)AÿBX!l P5 1*rXxP0Qq*iFPX '78C!e2HkYFqΑ |†)E6'q*-:-_|2Т+ahhUOo5z /h9   hW70?0AC`!!ആ׊8ԏZZ!M CTMkg4]'l8nC?8Рr l 2p?x>LyBY+)t>wTh',4P(АB& qH"?x57aZ1a,hC֐Q mś!j(WxVՒ;Hhxi M]}_/Jk#n^zkn9aArC3l4ˆ9l[ vMG~sr&d>D' 6P(@avZôAYCN ,i t: n{9 a Ab0 TghP d!m\?M8k9 R hP(@ơ n%h`kp&"b(ZAPDrCk9JBC/i t3 {5Hn k0gp*J4p145\rܺ5\q[7P04!'ܴ&}/Ip);5BCS9o#[$l$ޱpBCs/Z܀  ЀyaB0Qe`kǢJ} цN ,P8dbʐF YT& =d_PHqP mheg. ۓ 6^ (kEi ~nx'ho41bnC[-O#['۰V|3Uqoo{qh5P(@ 0\ d ׌Ҭ!!je '1 3BE r vFef5sQ5)츕Hp^-PV* OHedhi{^ͻDWb eX?y~2 1 .~P(@6Zcb2+/o8kͶNgtnP9dᠡ IXÄ y6Wd' \7D@Î-*\04XeP5Om8KK eY >kA3bS0:|eZ O؍'n!B 8&Gӄo 55  /j87ܡ5vR j zZ@ʐ*q @U[807HtEUj{K4؃gi o&ΠᆓV-^%6조 x){߸͇AN04ls58x~ahP2`K h@!^*}58܀Ch 49t:PF) C~hp>z" h8C^e:݁ !.9!l qL57AqTkZC&D a}(!ߺq僦?Fn8QpzO\R`w:.]S G[C ݹ!%OgtBYs6LvdkDCOh rK3P  (0qzf  ;8 񐶓brsP`)YC;'g451|5 R.z"C:͡˞Z*x&04mNKZ}a?_l ▍ [ *@7w974 i .wBC4P@|*!뱆5msCLiYCḱAit}ahkzaABCsR5[eO\ű5 1Р6zXz!/|퓖[d uB2ȩ6jO vBÿqаm Omp/$4 gkP^hPCdhy >'FBnـy=AP@ÄPZC Py,703hC5Ra4e!6~5|Q tBCטC-HeaoߴArޢB \$tϒ%&2H=HnxРvlA:Al ov34oY@W(h F 4MfҬaBPmrpo<$)z/5Xt1h8aLePc >SGY2|^ 2604\z &wX d VO$e@W8eڱ$)AÍ'ﱛP2ܱ=rH NQz4>xHhp΃5 h4@*AbZP& :, j_CC5sH@*2Hhg# u"_;"Qk"hŗ?7ݯ敜?|7{ owy甄&ِjI`vhChx3"k.k|458wT֠0;s?- ԤϽr  hIh@oP`ô+4ra507HkP0"4Eb4Ԍ]aPZxC  ;&Dh#@fgb9i1XhP@o 6C## a RNho:C{ؼOuKڼ۶x}7 A|O i iO7 h4@ CCak:7 m h(i }!rYCm РZ;58֠AZq`e`bSnY 55p1458FVhxT~Yテs^֠B 1 D jKg )oi2'.XnXZt¯ eykn 95 z͝5768Gk А*AHkN h 204=6Kp/Z?*k`b`ep͓j jnou@ʠ2eC _ߓ[MHe`h eE%ǘ-h}4לBZ/Heo._\«yW |U^ hKJZChKb Y@TZCX YÈ0:4 qAnLҲ蠎c<4_V\Ar$ l |Q7$04\78A~O&#J$4Fw|@x !e@@!9KV,4Hy g8T'4P(@aPhV@C1e 4et\@0Km8Cvk╡5GVk8ahP` .7_w`C ~ dž`gP>5(hpZC%C m| [3x/okx3{_t l ah=&⩐4HYm~,܁Kl /%4+R sG9+Vh5?u2CA&e`kM.B337M䲆05" ZìrCB]2 %~TQhP%AZ, [QHh!t[a7\%ڇ}֠F6\'xb{I_Z:B KAyOb{rRM N'4^ e ۛ\A  Hx'42HhX?qɊT ~P0V5Plqa!>`M)C54h9ԕ05\uadsO˒m o)PCHNĠ[ih5\CS>nG6\ʡ 4pA)!t7NXh`݈N%ع  /ֵfDN045*sRk@ !&jSdhE2BCe l nȋz.i B24h f8h $1>w`h Ga{ߎL5Ȁ!'tdAZ?d;p6{(u,[''~;'\wCBsQ %P7$&/y 1nr)A9AIX O i_#(aޠhћ&zZXАACOkmCnpvX m,_!` B2 y Zdhe4o R9W,o"(}ĠAYo~^s½oW:;7{^ru>i m |,^5>h`kZ|[-204؈z+7؜`3DocKgP.2042XhPc /4fi"FT,PRCCvk[n]yv'1 =6kf;h dـ$e W,7=tKrg04~y柊DC 72MkNں  Pe ج"8{[B}G`SAm* ohаmzϑ;A52D^ k1e5!h.r)[sH L78z#m/}ڷ7܁81 x"&eg 8)h`;o!HlRIP(@aF3 C[ü lEm֐L@0c  5vOȀLӻ ٭@Рr tlg=?o<@N TD sZAYn(Vp҃(hQLKZl ]$ݡAN- w&_9CjJTD+42-X$4t~P(L6MDZCUʐ` #*TaNBC28 33a6g4b /ݴZ;fqc% dDxjd+401Xhh={7WZ ^S>'?<2(h'$HgG,?y@AYAs gAZ+7`[EPBBT9m 6 RЀ  0'ЀVkPZ@0Slk{On ,T!k8uHb܁;,^~e@*q vĀ5\nAA+72hoߗh hg>`+fIh/GR߲&:M߲4h A)۟:A C <BB)`o9&A7e$[|%4,4JfFWYìrCh_P4C!ԇn[7pAYeO\t6SHzNp#Ӛ);sk*?>G5Ph3آT*ç^K^RB%-rŭڣb e:/\ 2T:MwOV AAO˴Lo =o-;p -gphCP@ hyh"SUSj!*QPf2*6n C܁h8.hpZx~2Ce+45Hh k hp B'e é&6W5 CXChq! 3 SobN ("G>,/=;N]|6 /5ra (䡦4rv 5(emۨ8-?+<@Z$V>-GfPNt[]Cj!8w$%bPМ C'>e`h`kP`4HhO/% hqhi*CIk&& *@P s^e ⴬a o\E~Z7o\mV-3pRQYóKr>Qތ+غ|Rw=Q ;;PL in',bpD|7ϸ.:а+{c:Ve g,ٖ/A\M2s{KޖH4P@C CC@lơZPP2  aZzđPb g nМ֍k l _AAݨۥA֙[= |^nQ}ʧ#eArF`7פkYsdc;7z p9p+jdc+4pބ7yyH_ nBK>Ɩgj ЀBfRXî B,Y$A>$4o]a&a*0oc  u)×YW]U GOrAYլKZs"A` hnyP5\~+45,;эZacjf7M+_Y !AJ48^4Cw?vǶ6P@~r ЀBfʏf51oi1F0.Ԭ B(P97Xe`h k nJZ!f8h|ͪ ۿlE~i#s'nھA$%.\nz48몓xNhp~:5v7?o/-}%Ꞑ]>eocAI<()IH[{:3(h`ePМ}O~_߄|e h@ s q2 \FKʡa&NnܠСrk(8RVVRZaZ C[sHhiO.J7|]2(x 6k gerI;;?d'Hѵ" nUVc, 7t[C//]`7%kP5c6]'pŊT?a +a p r{K2,jo e4Q58(#Fڸ!59X~$ iBBCk142HhhN|կl\+}@YCӷy=Al|]2@VWҼ=-6{o7t$כ:l@ KeljpZTg-;ʠ !-^ѝ!AVH$4ȧS4204R4DR;$HVgBfUȦj!|g& ?x:ժGѭaZ ]aP 괆q@ʭ _ >Ŏ=:N2|Ո!AeH)@K;^,4507H>=.aA񄲆xhPr~R{Nw` U/!P+P}.{ XzaJziB,IpV"$P(@xf&|РЀBf j@ʐf  @C 0 7 m9 BCU0q@Р >u(HDŽ3Yο= @Ac5<ۿ7Hh#}mTΒ(GXhw_sCC=h@1!A*fP@AN eРn#TBWdhG|g=p BTbG/yO'& LjB M,:5Hhh޾OHezWRq?ܺcXTacesOVP(@ Cpʐ4Q5$n;P(eCh'P2*CB!sK9Q@ĐV?b󟉁2[5ӁAmZTj >8 7vIh34鏲dT'4Oj8{ t6R J$7)b,:w h[hT>MBC[|0:4D1hↀ2􄆴N ':4k 5 `% |5Q~сSPmv4Cb Bl>[=@=ܰs^y*`_Xk hP;Ar ۗ2DwLjLI1P(@l@CMcYC۪P58a>!;7דyg!5T #Z YRn}QOSA 2Hhq=2gbhL7g1am/tK04^uR6'knAYѾfW@RσT;2ܶݿ jLy`:9]Xb/% 0P2,7T5BC%6k oXP!44D)wnZk޼ƋS~:.~ 7'h=\/P \x+NXRT"2 :=,_[;^/iل+]RZu~?ZhPAk ROLg@8e8 РP?J 4 ໆahk{BC2T>AKP4nP~wTBnH:52y3bBTZ]$1Vh* |$k\4Hn /x>ZNh JBdEL߬  {104\y앐o2[ m OU[ ޺,qA'4Ȧ V$O >ƦIDATh8Cm0-h .Ih`nOkHㆰ25ڃ"P 4DA÷nܡ ߺi[758"N8k-R7sЬK] KhP ᭞|!mClf:aHe`h kA7,|BKo] d oXm?cF?x:/.YE ae4P9aPkj+W:y047Tk 5 AFCZaƠ5 hhW4ذ 5|f^ |.AKe ğ]M h14|(AT/_Nh@ a k.(Q6h efk䆮`AChCW8 X (e`k`e`khm?e 2`)pJ6;{,:>_>YkD " 't4pNpTq>Z<;GxF"yah_Jh eE}ݳXh eAYO AC A4L&414 =c5AC=>k*07D*__= v4Py74_542A;_ek n=Pـ4W@XF;YxehC|oE !` s @CHX"D(5A @[AtY. g<)G*0w2X_P / AA)@ _ANP0W5Xh(hBji (à֐QFK0m/ 5@rR5LrYC8Ä& nh$9oG>}IKWnx K͒\r"OX3_f5'.z9>n_,Ҋ4NhSE"9C;vrS<e,44ooz5Hey˾g~Wh'+`8Pjaʐ` #*@֐]fA.?:V&;)bAZ0]hb 4W>iROZ%Zsh175Hnh^pb/?f=N7ucC5p%h ȝ5=[Dr! hN9;B^ sz*W\isg5Xb$ l D ahA$C&g+ateҬ!/7i 1 ܐ } t nAI+Ck@n >y:-AU2JBCz1 / ŁGo{#>]Z_1!k8q#l Nn%T};lAAj)O4Hn3t?>k38 RʸIl~P)C=qN0{02<4 nW4h Vc 3 }hH+}8 /~nZ|Ω;^"~-!煭5hl| jАBqýY჎4kD Ӱ +`q- ֏, ͉,)=lw8=x7-V|Z(|+U# ؖ"<͉=vay]hYh`ePW^cсX!k h}8^%oB@@@7)VnB ACUʐ!ו,P58_ڃIXCN 0ր85rQ*s֣zNHk8S|BCzv7N?DmY8}\U#y0$[64K|^I 7GN|{eh/\B`'A2Hh d e`h@XP(@C%0 P20WАjgP\ !  %b NzpZ9Ye74!P'wW9'fs[G6+|=Rt#Cb92L.f ^!r p` ~^ * hpB+C/@3A)  09e6:5!f>0"7Bw"0.7t~(ZM:[3@>: ]O.zK':i<>G6K]B_?Yw{is *ڠ ?0 Xn`hxg~|QuABCs䌃rEm9!i@BCsW6B2Hh%2(h`k h5Pb71`fec Ŕa h~yat4h7PnQ=DZ + ?EoΞU~f 羃Ejm5#tx50.t FQو06HnP{\eh ['lABJ2Hh uA&> > Nh`e4PqNkIh( Y@xzrCyk}>hE=Y=!- Z҇Vk@0`EMKlZxإBc04PmOHx?{N l ,K*lB5C hW Ԯ oXU}q@C_kh}]m~%MNݧlMs$k83-RZopKbn`k7q# pځ1Z$[_  o}^@hKVxg}:L- }^i <9gn`b`hxVڅW>h(7 WYU;stmח .pԵGA)Ze<a/tnjB wfщ^W=Ł$ DC 4Hk w捫|MgЀB*ip*t5 2nZ24Hn04sCd!` 1аOM #Z@ d03@9} r>?u/@g/ٱ5_^@,8./Kh+RZ{&X䌆 cKIk e%}Afk%]jEL M;d&,1i>AA/Ae 5P>ΐK*Vxk q!g+b7/{=jW* }> - h2Y@)7Yٕ({f)n kq.\ A*#8@\X2x gA~ -Bm)CР!Za8}YC J*Cahmc  [a D hbZ@А.=nYD˴֕' R[0l- r|akxYz?v.(e3׏_Wɾ >D-kͲqUjȷnZV~AA$ue_zΌPY 4 gȢ uZSrYCz! CCyk熼֠ 7?EDF1 /(k09k >oP( %AB' iѻ?oo|Vǖ'DA=!F<24!K e@e^X45p)hX΋?qKWrtJ4p{OP&z*ôa kf+X g hP+ Г,:504! S3F (h eEM4z]*iѻyZ|K[EZ+NXbhhN l 1$Fg02FG;W@ ~04pvA)ݜR/\+4Mu7pYe$!A1HF"ޥs 4 4_j2=jS02 ܰy ! 78!L Y uA* h#Hex&Go8y9q1"hؘRI@@@S24w204dQBU s *wn'4H`e0A[J4|5ZI x }"b( hi"<ZhhU!9@P2LƵHn 1q@78!bap0𙋗Ԥ w;N]?zsВМ{HHXhIb`e b`h}n'exe@6k |ݢ_E*C 4޸">)4XeA]ЀB& XC2Te @넆HkWFnH>qNzrC'hwOƼM|i!8u-zƧn;O"bg( *kP:82rB@'hC ߾iAua1(h=r+!_CP(@pАWjNP5\LFWz! f52]-Z//8ܐ&m oq@@ AV #242=Cg%2Uz7P05D>eƾzY~.t8w@Ln2 BÖ2DSvL镀 0ue*C#0&뤇I@CUㆮ x#h-EZ& $Hx '4 7G~b ԟGIDTw𷞇D-by)h ex-Yx  vMlU+ 6 4b05$(C֠ 8}0/*zhsC58 s_yC7  S  d r4TUDk=H"Rr2--_ppoh՟O{'ȝ& 3l 2Ѡ#>x)^gwkS2042d߄SoB]o}5 4LQJZC.e( =zXk/VeP@ *C֠g 6 9W;pC2DrC|̡Z͚=kPc G3%'/O}uy.|Uu YY. 9NJ/̰5Pæ&.NkP@ח4KРwMW6( h @o a8kP`!r@q'407_;Y?АqͧP vNd :$pCjI:ACP803}87o\Q@ iz#ҙ\PР*ԸP'4E,41Hhܴ֜}t{ -3e!6ȃB 5dE /204EYb Գ( tP䆮m98 fkЀ8D++yǕTаyNh§! 6;b ;C짵}XwiR7 >e24Hn1 4DoH47 hs 1MC+02/Eb2t3"_F5p:<!!spF54i 4 4t V%CNh  ξ sPYbͤ58s j|Ri s(yrӧ NhhN|ϺUZh_fC8} 4k( Zpʐ a_P{ 3Bjμۯ[p^=]ݡ4$s3核ABCX4Tk 3*";[Р^ 5Ї>VAp4k Mpc /0128C/R hի 4@P(@C8C 05 `;# _el@Ceo ]p0QeĠVnsCCf/h4L =TV"B~bpCxԿv1kPРZ$&&[Y!K|5;J4Hn bA[+×>4PM+D!` \PĐw掂gNA)C8C25soNk={ Cs]9 >tTdh +,4쭠A BZ78CB c eSR j4Yi *ԗZu&Xe@ }>Ben eZC/ 7ӗgI@h+̯8bMgnwZ=ﳂ ݡ?\_1O =k@0N-U|l t`.{y:3` zwB ?o/Y)-5^,WQ hɾ Ʌ CTbC 5M 2(n0 jp8B &ZAZZ܁AdzHNePܠà07HqH!^k'hqb3nР}U;?YnK:>=!/"m/&&bKeUS<401He/d4plAT /#Ǧ: ]C *XА g≡4! Z! azC +C8v KnPPlpC2J?4n 4">2RqJ4|e9N+jNww; 8Hb10RS/h9}: ` rVB}>Xhp\ 04&FW@v=N@C.eg 0(=pCB!|!:C$7D^FZC8$oKIƵ ]g NnpN| oUww lP@G}<rG`O@@lM}`)lD8+V7q2-;KW!J/]^#Fja8C oWY6n2t JXCnn >8RWehu"6`!rC|ʐƲMJ1HhP ?A(eʵ}l NnzoI9}=lopUC"ZosJ7/RMƢ@`a5l+も4Pq@f7 nG-Hh}S:"SӄO:AC.ky 1bNn:! b2LP|& >bcwu7᫳@o鄭ﻋاg@ btkoVSΩ ]H\,i q.~H (a!P/XhgXor2t3(e3q`hy !t( >wPܐQE+7" NhĠ?'2BCak@АAYXkabTY³VsYE+Ўۍdq n~Eg9{՜58UCD@֠.BD@h@ BCm0:4" Vy34Maq08Gpw1f$7ZAA 2!#4 W~kK; ˏ^8wkG4ˇ rMe4XC4204ȡ M}ӊV |W~ߺi[[ v޲h@ ".ѳHee@KWTꘐ fu㚴w;IP dBbV!g q>gH1x wsC;pOܠС7XtQ:04 d  h l |ЋX8aiڜid4 l Ȑ `dkDO' x4@ }oKx:WZ0H 02BCD*CC@*2BCFeh5/zZC[gŠHwSB5$COk naPhg4W<%ANL^$7ݩ1 jZds;.X`+ @)g_vgV=-捫rB 4AC2t4Ve SQN0b! eT d !W >q }.:rC2 Vг"z2{_=vAB"}D("A"458cl׮507MkhIpl.RM_rE 8nD !5 J |0/L$Π!C+48*ZM ΰqC'eȘCNeh 24w?YBCFk2 Ne  <'.R;VZd |oWyFf\s˂Ă?uIei{xN2H!Uk2$1(hC%`"B/9A)"H=P ] ae yAUA} CR8àʐ 8C}87|O+=[C8SC5pC8tePrY`\ i)$4`kP e:2в\Љ.Xn_+aS߽yMrOԏ7oX&2|Q@f E "q Bl2>3&22 MA*C'hT042r:L} 1"š;$s aqT2+a O> [h9 ,:;%g>`=j~l?x7-;c;S jA }fAB 8x{n 6( l|ݛx5P XnCyÎJPܠ G(4 塡S4/L" J*`azH!:Z$Mb!yѲO^YAPTbe`hY(!Ri i't U)/];&8 >eOc 53TڠA4Ъ  5 g+C4 7? kH ]'C88S0CS#suR SC T;y!9DZАAɂu \K}G?E˴  N_c& @OH25^+u{Jo( >eA& 5АW(c 1q:7gp*!gAC2@@>tVsӃS|ʠ?I1+ONV" >bPo cP^ 3jAÇ% 2wܸf2R0bZ$҂ >3MuO!|ILV2Wŗ5v d`e`haƠa8e|AAgN߱ѩ &(C5d> NࣇeϙxqS"!-p_?\K䂆,15 hCpA&|`zzCw^Hh`kIK*?r@CSz@> y-DA& jhP>bSRe+ͲVdl J.|@BMa&!2 ;`!ȆePP[FQq >hl:y$*q,fiI„|!dAxec }bk@АX.vL ȩ?|>y%e87P~iL@K]p}NpÓ' 5KH~We%9 rwL_ӄ$ j@ SM;!c! >nl3do(7 1А+? A!p ]F<7CB?a`7hm bpvL i15710GH7 "dk}3BGHe?uѲS5ȏ}&{"# i5TY{pB[A ['jkLD@T_rЀ44!aDhh4HepBC=m26MXegP3 wD 4 mH 6C8t.[8kAVE ek Z 9fs^՜~ߙ%45W->yBCk:1s64dlSN!@B/נkys j<*+R;wNT w} l @1 (@LBȫ cYC-d i 5D) e!F % ]A3c֘o Ɗ4n}0$.! !BZ@F84Hnkg.^}%4M׶smH04Xn׮[+d;MB&Z 42P2P:V+4$W an1VNT!ׁga6 5D&Z!>hHqsO;)2DNP|1 !Ÿl`eh>J8eO2e`hHPq RMLC8(w C8 V2AP0407oPKAB-V]t© 9v6@g~eRWW842&G }m"mP@Р|A)CC`eq!DI5| j2+Vk jF76$@@MqHh)C8C m޽b冀8Dr "4h 4 td%A4z/k >e:[9zW,ZL%+i# z%bAAwܥYOǞv gPS2(b()qЀB4TA5wȆ>7Uk!^CS4(Cvh7ڐp[0ܡvЉ|ⰽ J88wP@Ġ@0n Z[Igk;a._ DHHgA*R@ôp!9P[9%SdD|ϔМ XQBX04ڮ6􇆀58XBCʐg)Cvh+8ЊG?mV䅝Hn 2н b!FM?x> d ϸ.1\=j*@KYA Sa|;\A!B60i P!/ٿr*/Დ ݲFKR. 2݀ߊ(@aDe_gkHP'4rCkЩi§ M Neg0PahC@с8tM7 dHV94tN45DC$4HkPvJ-ǂq d׃mIМ~.2,(eP[nh͇G # 1D&Ԑ=A`AZVe t+J'K* q銍3H}oEa02d\W ʐM@BRV2\h[rAZ|-L'P?1AfvXX$4l\P[ l l vLqoQڼ Qʡ h(/ d02YC!NhD:YsC_>hZ[ WцsCVn)C 1ԣ  <-2gC9nlXZg!ex/ ~b.L"0bO< XT0aGD@[&3(\޺&A}Mo+wDPC$2@C1qC+CWhpʐ$7 d MhP ZvR a7AuXĈ  i04 j @(34 V~Wq0cv-dǧ R^r$T rUe famЀB * }!8B1e 79&m ??Z?PUah!O1h#epc?UI@pCwDNב͒\q’\tO%ܭ9-1} ;{*Ԡ^;?j  TX|A)PeHNh !82t0-4ЊFCqmQ04Qqa 6 ɷ5'ak42xVs,jxCv7K]U'-ђМx޳ >}q)å/QyNz lvV\ ,1~$4J*S AY\LK&D1tR_P^"!M5cu5!>ڐ6dkʐn ClXC C5LBC;·2ڕATLԌ&@=>G6yR45ø0;kwgfQ >1Nh (BPpunb ( U48ka8h#NPE @QXh\C+7>֐i© >hX{o"F (F!M8 4 u)#?BefKf=>G SC틶*Nb(pX;ݞbY"D1 R$4>|AA|Đ BC8XSKb2Hk]ц076N 37M(M3BCOe2F [à7{֐e(CIe4fǧ=)iۭ'.R9?|>wx +dAMClat1bsmx5i+Vs{)AZk0Vaf1>z{ [z%|UD]˷n\Eְc1oY*-e ?P@C+1Ts`\]АKAFܐ` h `)X߄R)BC62ci @2L鋽 ]<|7@rԇE]lA)[W|q=c f2ҙv Ć;u:/(eP AA:,qvO@Pѡ!L BOjP'4(kql@>" 43  m(@5l)C.kS ;A%ccy2gV;Y e`hP ydb=(ԈJ k5(CPHk,i ePar'0 Côq@C]`•54/|"-C{#h9'(h kxރw7KiB ΐ %/\+VH,48I 2KʀB 1Đ]* vO  t>hpFo --CZhȨ Nh@ahpP2ġfeo @z|}PgIbP]Xќxߙ%/# 'H2؛-a{"F08cK@/O4|ʖYh<S[_fVs7nYS+͆CGjh'P4~rqѡ<4`(> BϾ_0{q0T =Ѐ @C2|˟@ 3E_՜> ]B~.)Ah_8zvFX_Ժ=aNd/[! 1:,Xheyxʗ7L}A*G&B 0D*k e%mц5!$4dTp0DEv2ġ6ec > we'7\E ]Ͳ/AA)e/8wՇF9Ob_;"֠>,;4(k`eˀ=&Pq!VFB2!3  khV Ne7Kf ]F5h AK@ Y`h 8ABah`kgv[r19AZRi ξꋖyAPЕVqA)C5 ;HY( wpr_I4DZC 4tU Vf/05LZaDeHaaAaYsCŷni,yw _8+;L *`# _jK[Gz0`ChWL U҇nUt*A-3.pʀB @atkp*CWh40.4Ѝ_E 7DBC53Ч̒2aZЀ8aecO^I^Pp Kwps@˩ \4_g  tT/)#€ Ch`kOK'4ȼwoYSŷx * ]2 >e y2V^_Vnsmdh(gCC2lI̘2_PMg4 | C֠A]'.5.v1 u¹!%×#hM%dL4 򏱄/o@s?U</C[]ROrv@נB^-QА@ 啡<4!5 ; 2@A&d+4xeHqx CL! 2G@ '4Hn*D $1Z.)Co`&2 o/_:K0b@Mٶ 2_?zY2 k`Ze5PCC1WЪ ACZG#Ffi 4k45ZԚhakFWK P71?HpvO2 &6/xnV! d @p Ո`) S?HK1pA%hԸy*[jPAYTi >VWEjhJ<4(Ck ; |MHkHP J8i 109k}_C "A Pهz3"|eOM|de[Z*e _': jN}{B&4|d#gL *Bw&54?yfA%#lYeP1P!FW2 !v 58!@Cx<!2$@C82@C5L"0Eeh i_o1q Ae3ѕahV *@C퉆O^}HA^EZ1j49!0W4Z$@ i`i r5AlC`AQ ֍k|\?1F*%>rr`Y39!Aj~qZ֑ ^j @C| Y u҃9X2(pMAß]4qH!QN\j/腗 i (`e:)W>u[C=y&wM4HkyFRG R'4 5gK q6X =ABÈ04)C=Ȓ6(nhݍᾩkHS V,4He Pp0 "C$(ȣOZbhPR:e>twoO1 "f0dT!zP 3xLdA)[+C-()[+bŎ5 A8zD:>.0Qk; [1#"5OfrCҸi ʐ%Π XC:~N1ZV5 Pgꓖh"֒|EʠAA%h`6s_vrhjrļCu J4p L pS[r؊":՝me5Pɭ 28|!#4$5 X 04Hko >k\!FA'wULbDk(>h@ahX?8` ?mDSHEYtG-e@ =l]c/3:XJܠv*zz9+b>Pzg9>DgFNT2Pаq봓Z7䂆,05 yB^|Р!!Cqxhbܠ|t>q'4gt=H,pF L8(k(h4bC TNh eP_lӄ&Vh(o # 3L ]q!)* RР%2|e V$4 *  aChȮ !|[UA~ G$7XhD"A%5gk鹘sSqkxq7h8(?Dµ_y"/:ci}%hk ;#Nh`epBO+ O߮f5'GAA7n(=Q/*1$?ڑRAh%{% quP^?!r^…2PNР6 Đ CpC5 UAC DZq3-RT{I􄆌q }a!Ls !^5o05H_%?R]{' l  4Y{_ó2P,CZx!*CQg%zAymL +jmOVX  rc Ck 02[ǴQp!ZNk(`! AC!a?]: 1C7h2Ї$:AРx>z޲mo̍[jϔʠAqP1 C.I8_= m)hP+Y\2C?>Iʠ=W5QE@q  Aڽ*WYa"_`_P2f4L41P04pC$4S땏n r(C5 a 103q[CvZCU_| e4vD=UACx$1V{7 ,7P=Aq /ezRoޠ`'q?Ebȋ }YOqo޸C7V D $407BX;;ɔŧa (\!"J*Cm05D6Hkm{i ZOAACZ!;4L: 5PU 8ږ#б Ȇmnj 4 g4P#/_v6P04ȝ,Xr] {n^ahH#  2 C!>hp*PATM4?_v! wT +Qak( ZCՎ ]!2@@@k2$@U!aB!d56:{7h@|;^#gi)˒䑕 O.VZv4o)J4|{k,O/dyŸk g}Di,1Hh!2ЅQ;&2Xk e% (T|!;1xn@(P4b 052BC8@P'tCz4ƅ(a(xNj1704507:1 o$loY>7@yk^}B+yhP2䵆3-eG$4HkE[5!K4>r[h SA2h@05BGM]28+ 7!!P̈́/ԠFB*q? ݇N gw6V͝]*hWy. r5Ї,8Az7 UFE $7+X022CbAuLAEAAC3 ÍhPxߓg4̑2HhP Asy [ {Xk`h ߙzvnt]->&jT —h /%+1id |g>$BPK >hA*>0:1HkZANpH\}( CChA@̡u'@aAc6` =~/4;27=w=gio)7T wE2T w/2ȷ֪|P J%+l;72*]Pb [ fAh" UdhG؊BEb >k/?{bU*\P15<:tY4UӇ /Jh8K̀$ex< Qrȣ?Ү JJכq RBw"'6P۫bepr+#_~⇷ɥAuOP@}* C[C:YCCА` )~h >h7R!5d30:z^_( 3ƌ3Hh%Af^EVЬKkpf{ ?xhoB\(Hxe,M:2kZ4'9)߿fdgʆfCPHb ࠀ2t2ti t&H574*A$ =rbÇ D h9+Hu*ˏ^hU8A}!vCtX 4 7Z_(<2lA,5yTq+bx__AZL1`k- %V༷U9.F2SB'҇Yv9 R/Zy׮69Vy~mL1!΀B١S'4: >kP5p!Z!ZNh6Lِ>}9U>JXP%-Ң=vOdYo<TǍ>h(3+a;#F߰3b6 cT-pIh`k28Aq /04wm '4d\֐F aA( ah2[C `!"[b 5CC&dh!~Lyf_ BY KZA G뾉˞, hl  Cr]$ #Cege"rf9//B?s-4@'7  2rChI aCആʇ2YCdAYC$4|Q@Krԕ!` S'I[/@' ø rqe K\zTs^p}i֧?>{dF֝` NeuN F$>:>ge6ēX~$KӯX+d C1:!#E, D Y7ĐCFQk %xH6 ;2bN5Ȁ @2̌58ѡ$1MƉ3ar+ASXh5Hnp*_t3?DwlwFTB A= AYmVs+׬rn8Aݜ򡝾 3P3 Y BC% f@lk6c Аf 0 h3F d  P@C hÕ6P*g+61hb@JBC27tPz|0.4%eQhC5tp_T2045GI5~ozEr+A35Zr@'l=!PА 01>F3ZsC'hb :k-y* OV @@~Ihs.?aI*.6Wv7YSDž" G`v^@Wh`bpB]`3L{j:l?ɷAB_9ĀB 7DBpPZ*ƂsE D`d_?}h58 C(:a[Qs7h jOXHe8 J57A«,4eCÇ.TE y@JHh`kp^2P v7A.2XhQ)M ak40"4t 8(VȮ ߱J 25p0(4̤8Lw5 3 SZ!&l{;>] h5(eGݳ^+9p|T04|e֏_ w÷n\YVT QJ fhh4!ѭ!F,WSC_%j34kw~tklrȆ,` ehAl Wċ~e3,s,y+Jly}lwÅ !2Еí"'Ar5(w h`bHq? |7|.ŅB nPj֖ 2 krB=zs*dWn綗;6uq5[Q5h( _;I NevSޟ*/ΠA-X80CFeЙ <-/+VmuT 2(kPb@ CU 8@AYC^n1745tkow;TZCs'~䬡8Tn wo k4fS,4 Rz'Ek gmXs1 "h ᎍ @I>2ܦĠi {& 5C.eqi ف4>_‹U/wqo޸&w j6F 2 V,44xhЧc 56 i XCsOJ#2ޥ)C a eh@@kkh 9ܺc424e';4g NVU\2*n=ߙ%#@ h٫U_PKf,b0 +CvH35\C"AIkLkC#ޥր Yl8gi κ 舋e`hxwك4 ?uM^+fj&Ae  l dpOXew%1XhܠΦ s1P3 ͸P5 6 |`AYC<7PZC{2qLFkZ@, (74>o?! M]q[[^EVePc qkZC,2<,4ٳVGfj&CPq':HkY1Hewm+w}~zAm_P(Aڧj?}rU:g SdVkP`!bݨ_걆{7hYe@ B212M]rܢ/pch%Y7T!PwQ(aX҉z Nep>mWӟzrq * OzY߿ _eDxCvˠDsiۑqxoҦpAZDaBT_Π 5Hb3qU58AIo*h!4mT;[ƍ6tHkV!WELak%PF(֐FaI@C9h`kn8yIBU'.)e ͒I.A@p1 `AAUgѱeWh5L'Ue6;b L 8 A]ʫyW~yKxecG201.4C GF1ahCWhd Ske 5k?}hj*q(l o. h2J7MF8pK7Jn8i$=ve]$)yGgkVxL2غ)w,].[*h>4gN}&I  AA9AMyY >_*RX 5]hJ P20.4! D!rdô)@Аf N[CP(0muMfi r3 WDE3xv|Cv/01=l" T^:_0nbGv~Ne04Hkyi z$_,@ 5 hpn'EF@D'h(mHhCk }7k @IK)ܐís~ :ZZe32a9 a N֭^IDAT 7J*C$44oyH dDAY6A)?TIk`ЀB 4 m( !DzBCBEܠAA[VkpvR6keK i4K4tJ|V7Zex J k`npP(ek&e`hPu퓗eGB278 Ozeh!)|Nk`np5HhWe}A` (T?剡+4 m( P D*@ܠ?sX5ȋ|QABà0cPx@"PlD04@ %KqC冫tAs@kLGD蚙^=d'uTy,>vgu?ib q TͲ2V - ;7DZ+eD 5!PB CC4 m(` Đ FrACk- BbraVX!Ȇ{[ hSm7ѷ!eO\eCjlX5\z⥮8CS?bAB9 5VfuΏoadԉ2ABC8`גZA=[}g:V՚e8#dh(044$+C4 m n=CXd ֐AʈCF( k4fZс`e|V75܁e nG-04 R9RѦRyZItgZb GrZw˗%}$k bPaeI|B$DI@ Usƾw4 %,C[C^n%P;)~5Xhi AšԠ Ѐ @@wk+CC2t3R u,8an:NtLK $7aa!f212W hOq -:- _n:&5|5Zq=B;hZ'ڐ30(4 m [i#Z(NvP 4lݠo0?C$4a `ݹ_ wO2046 6:8YTJ ~5 |# 2pìCXW 5l8L D* O )ؑvy@@USf5'rͪaeC_}BbDzZ@05$G @@PBYe !3!kP eb|0fZk0T>G;jҞ-dm2/9nX.{RZ2 '/8e^ͻx3/ ) A4vaPOE@ U4 'ACB! &A!!P P4 a Z Y8(ae7Ѐ8a6!,nAA'-28j ePаCx: M eh%~ 2ȏڽ?T j gAnNe l J,0hh N_ à)ڐ eahhm(  CS m `b0((􇆞Pg4T  AYgy5[ce ϲN٫Ansh lY{hIkPN8 5 'v|[Ҏ;AͿ`ewZ{ 2K^5dˆ4h@& ahhm%!>P XàAABCxH !&h+`M!as`a-'g4obZ&o(CD@X{SR`> NkpBÎCenfOW# z玝gJhwkt^ͻ?Y $7H_+WE/Hhhk«,7wXCq( цHh(I %!&0 4h ZqC5Xh8֣ teWȆ Ѐ* V~H.\\ER6YC(g7zaw[^I j;%DBM4V?xfIke: 5H__ocB(Ý׭޹5{RC`%+C- r lr wtHXmhc҇پ_6.hyCuح(H5t+|=m \b,H\pAnYE AB*>ojBU[Za,e( høP[Xe}vCFa4@ sha ?eikw% ;68~񲭮 #Ɠ45(e&Z=/1 H{[jVNQ8fd b~d\{J_P%AA^;_O+\e+Vp\ꥒ$ >e`h`b،0ʀB!!F CC8ೆCF_ath(o !l {a@o >\W`ͮph@  x)e"Q᪓!) B)I HH!YC8t9b;֠x6y=o!Jo._iJ44Hnݛh2HnP}W'GG;jf!,@/qaDhцȆ a ֠! !`7Z5  4̵2+O\rB+qf&?v{#ڏ958!^ԠZv:4f8C^љqɊ\t57(hׯ_@YA&$4Tμ314@P8 haDhPцJahCC5PEBCWkP[QHkP`|aPe%0%A h x>^  HQ jQ7! 5Ȁ|' Ar]j7wvrP;TAf88|AB@çŶD@ÈГ|@:!ZCcᮿxC ֌A@1eM4p{hm* aև^O%^wU[ g8c e l/w~UP5  P@\CsRM%R Tv. (05ph_b~f5'|#+䆱vdbY3h>wObuXC|#ZArÈ05 7 h_hWyWu&xt%1zUy.cl f3!BBHBB Y 4H h`6P2Vtk7bga*ΝRC~thҡ 45 ) \EVРRAf 1\A 6k C8 adC%n5(h`nPҧ]Ln@E%k(ـ7j .3YYOthbOt 12}V8X2126eX,4- u!!xe1cF ޸^ 1Xeh}  Ak457g45({"x`* RB ?ck5@Cz_(!pC?֐  AkBaPP PpkXSdп2:аz!/gV \o /+  B PhȚ tF #JC  v X)Rg4R=A m^kТ5Ĉe 0~!dH[Cf"C|k`h k'4o   ^-C`Ac qxin̛e8CbMԄ20=5XS^?r}K)34@R3#CАz3`\Р+`08.xy6hHھ 3 8t ta06t\&B3CJAՖ5dF<Л2l BB+CԖ ʀf >u RD"<}QAZ'8de8Z'J jdxdChBbУieOMm{6ޭ< wƦ6L =a9I&8% Y[>gm+ l ДS7ZmА&.!1ܱ;h: CJaV-ô 7 Ԑ 5!=!` j[C 7t gphphډրN h*#!c%3iB$A~`{eqb梭i #sP~t5ϡ,@O^i ` hL!  㗋8bPc YVkޡ5څq64 ]@CUbh8 l?E5(hQb * 64oHz68484x  ?~!.W9AY->32XhI )xa>4\Mb_`e 8 qDؙSa*ѠV0 e ( E=EQ ih`n`e4%5?; ^^ A#_h ohjr!z8 T!a PC5pE k@549t ][C\V/4}vkih= {Y0 9 3AiPE |o!11t`DpQe8vD/tOdFc@h^uۋ"u" Tg=CBC׾Њ5tphC)4C00ؑ Ci{e 9[֕kk<аڡA5JXhKne45#ތFO85H28 Rxy . Lg>qYj@5Gmk2}1ң"MYePPAY[ԌXS!%޷+.Ĭ{DCX⚣68|wal=!h1|,+ș! *}m_~XkFP—b'J[CbcJH:; !**)Lpuh@C+AJPz8 82!f X6(khV(֭ hpephph /7R|1 d {KA5GmR.Xhg%<2t[߭q?W0VhA&hråXb֠!em?34G_`5=xXZCCC+Ah/t o C>4ԶaGQ44e 9b %цLkh=ڰzb}_ZCCW'А{N^(Ѐ\(t&%+pAJL9웰 ʠ! 2S5`_%v= "A| EvJX l * ߠ|xt 5ͅVYq$omX] kOr_MZe584xy?U5F! C hh=г5tR1 -(h5XhY5i +U^Z44pephXނc 7['x|#.X@bbQN@Tzߵ*4H\[3ЀMxiI|SJrCC. ! {6'C÷/ Y ?j'^b`h ^^˷zB?А*YCwP ZñVYz֠(z憞a5Ve CNQ@l?ȳ d Ơ&!7k3A2(h`n`e 1hf g`h~:T7h<>=!v:f38 ^@QpAA204CUQYp"k&o^^+T[_j:iYwPU2 C hqEo!XŠOY۩5,m7{̡kP} ^@cgoyl}| N,\ F P'%pAZ'01gXA 8ri+1nazC[g~/РfX<~vYo}dtlK|PC^˿x]Ɍ+ u@?g^@^V-4ā{U.[crK*.% xuJ!z zVl>Jx$󟲶5/n 57[Coц>ԆWek2oF`h/ 0H SD ~_e cS.W_<<1hQ$*nD = QY8*|j&X|k@CltqCe, ^ X^C&7 ChhZ 8jd+@C֠]k@a0GZ384x{`w,n'S'5Xhi POw*ѠOoiXxets2T;y ½,& ? lG*>oH2M/Iq)h`k04 ?^25xyNh@[skB>4Z >aVAЩ54 8F!ʀ׍/Xh7|8`aц?l&fdO6  h(a$p3Xhێ[P}L[1/(hTq)b h@F)4q!j U͉.>!oMJXkh`kF⠠e  l IAA! V944&3q,ԀP)!7bݣ(Z!gPljWk Rsj !**Y5 h.450451POpCC'Ō@#fl3 Ք  y9ːXBywFygOڣ(-|;1-n?r+CEΒ45 gFĊ, ;mAA[…`ˡ5TB+PoZd=q8%͡Okphkd!?ΰ߳hHX@CGPcXz8 k`nZ h οȱ42dph5 hNs'qHЀ7 <2h]b4(kh b &ʐ'_(`?y24`Odhrr  s+V4(C5(hh464*ڐpg &TcVkXRkhTQ327A*h smh(U5t4a&A`S'-j> /RS'U ֠ ^{ԆLd/ZA~p/-ĠaD3KAat+^,֐'ԮeGp94xy94԰[Ix>ך^uhh,4PV5CՑ Vj@B UahnZa%YCUq84Ĭa%H9"oY2p;XK͝Ų܀/lrPrxHtp;73;(5&4G Р>W/<@AA./57(hekhZiVyW75<ք2Kzyxh ֠AIXCk&|Zd&4/B̭!Ri eACP8Ԑ@CCkBCkxbV5dr>4! sdph?k9r  M O[A5Py‚XoxG'"bϞ5։>3"а%1 K hu2_HYJАP4Ú 4t2AgׅDJ!f  ^^j(C)4p̡!xW?G╇<_Y6oѾCmn C0kJ! CV ckg5$ 09̅2a)CX=O9xB(%l 6S`ay0 R?.jP eP0uD4y$Nvtᴏm)V1BP PW}ABǶcwdkh`k4Xk?*YLS. ZtPzl CX {C` <P5+f + `-[ѳ5@:4WC|sCPP  nۼ(,4.k PKePuCT3ok ^_;& vLR B PԃqWyeݎAi\`q۬ 7441eHsCPCCwUt 3<"'StT^̽7QvESR7jDskX3ؗw`qROב5x+  'b,LhPրs1 A44D:Fc xN.h504p۷r[A! r;&eԉ/rb 8J2\1h45(hd6ˡ!F5ܐ;C&7 :FTTMgx3hm *YCx`! g{> _LIġa%X #'x.  6_a .khPQXhsé >~B,k 3nRXU@ÄvEž AAA,,+%zyThh }jj(C~atZdЮ86kCʠk`hhh =m An`t4#ho•aAaӊT &KYCqùhXLh=&5Xh@!a 1b8đ/eex)4,9B64204Ѐi(-! .|qB2\vum+-y,W/ m)[Co" 3PCVġSh5@XZ3(hګ]kJXC?O5(Züsڂ3 $.w!/(hkȄذ&GK\F鞨 K)@[NޤkBGZ(e#ܱXS a%4H3CÍ_ߩA^5X7e`h`ePР Pϼq&A//-ehh z&54 9Ѓ2A34kՅ8kXZ mAC=c=coe  -ZLgZXCg'GthVq(CC?{ 6(5(n`kUO*ߠCZ' 4,<ahHϹ}km7S=6 % ֠fC Q@ǧX52UW#  9Ѐ?VV@s_=X}83 3WTeq0exIr,Ծ{7.4t*;th(eg`hj p1g QxHm'7tfh 6V/6GmРB  nYgk`h@! s.C ` j^oךᕅ([9eH@e@% Wh`n#!En ^^ \ȿ=F6l<{YmCsnHbSt ]Cmhh}Cã(2045M$2)kZ h> }YYCCh80 6Naq@A!2룏nO 4 60X!EuLp߄5ˊC32Dc3ao3& 1h irA (//5Ab_  OCth蠸khhQZc! JB sgH呝[N(ր]'b'E13 d! 8 8ê[ 6D!?0hAC)<0+bph$p[PZ,{ॄk8Mݚ@a2ҴQL!/Z1ĺjDb{JP:*t  oۦ *hxO/މ5$ڐ LhrCCmno1C!a B44+CqJqJnmHXCiaPЅ5;rb*F  mr)EPhݣ/9hџ7Xk f@$'$?ԳJO' 8BWN7 *" 'lj'S?؍/*Cq(zP2 CVޑ+V 184  '_((- /?$ 0|ҾBCdLeH5Ԙ)(OtZ F>^]mf 9 Pۯ ` />hؕwS^^BC5?'1>}ꕕ!-C*Y!FCC蠮ô2tg43w/l0 ?[ /'F?֥Ͼ YZ& W$4߿'NzQAe4͝*.|rt oۯA{wb H4$JwU&4@Q:1qe}?e Ahe^oA! h.YC[6HeQ`DqK`yyu Ak-7XhPd! Q"lrCs13Z6T9td m243$EOJ47][U_1P2I  5P  ;5T{)k͆rFfnA>9_#?Kw 5! 6ݠAAACqo BG.+A6=4>22m,2ĊhA^h`khe O5Ģ qC,`}! 6P7Beh 1wJc]CC 2AAC%e8?&!f _9 ր5䰃|I )û?P,nPРƯ5m~cj`A5PPC16`E~ἻKACq񝗅@k`e`kB+C5(hk8[e_jf4yq#Gy_%j o>qcTrApAA hpk 4$n$RedCUnSCڍ9i 5b M 5U[]C*=31k;Tа”RaZp%˓ڥ./9H[XmZË 4kJ-k|bcf.t"A#n5(h(.2ҪBXC&45C2452 oK;k(.|m&{Ā)&;r9P 204!akGH^^ACf Yrġ97 ak]kڮ@C&:2&h`egh Mk\XCV 4d-184d݉ȁǎ.&xn띰@XֿYm+U(ؖ>]EhPg%dvODμe ' ]nN;'C ~* b RhwB~VԤy| I^^@CцʠXrCicEfxY̞84YkP#q 3 5 3i Ab(Xʐ]4f7w CCBԍ熿 k hXuf_?irhy5=ɰ P ޮgd^' ne k(^qXCqm ][Cy ieE5?>Z,ђW+CJ! C>704(qPc֦`C ?pmk5A2 3 844kXl!VgD?0o,G}p3AAFoA3F֠'hX[s Mg(!2# 蛰 ֠A!hGDzA>Pׂ5|ꇻy(h2aw[CoSAClHZ;WgZ2C37Xhpk j[C܀3}RhrCδ 7!Р= h: РVGP)*)2)3qg# ڂ4YZj Ê!24fO) _NmG7PnNYm73Gs6yb!ʎsFx45XhP9K 9 52AAZhv ~GU8tg ihkh_C֠PQLj1 {e؃*WoP57!p9ܐ|n&ܐR-BCmqhb &bnO +,Ж;,1 'CN &K`{vi| _ ége]$mP`DʐAe8Q37BACp!qd^ʇGVbhC ЀiDC1_y>2AA pb8ܰhNXnO^^mAC"7>8䆘8TW<3p ;hЊ55Ry4!gPg*@C@ЬjzF4M@^aj -A&,]o)[k~2agn F23L~1,]8č '_( p׹3e??/{xŠAz"2H鶀57ȏ-48IdB=|bц=2}l,2Ǎ Bְ㵽څv-n>[N 8(q+s!v+7VKP,|?gY-D(vt5t =koakHDT߄:vC s aLͷ0EaaBCۋe Cܐb 44 hxh'BB A@'nh@D&4nReyA)d!5ChFvMokPwo,VrFc4!#,}8ܑkx uo T #+q>?j2*7xRnH+'S5(qx~GCCBjb1qCG`8 e?:4t4Re8Cڡ0t gpbphSxKޮAݍo ex{72S# Ϡ21`(oR,ξm!vf0/OzQAu.WaAYzZA[=R)C)4&VY S' s8Ƞ orF[`}7n Bqˮ! obhk`b[e[ 45Xh̀WZh+x.XC%hB i&1! Y@~T44\veL o?spN ŕ,` b3b}n ^^]CCGЄ2oUP ,2!G2[- Cm迂:j2TjPАg4  32=ᡆ 1ICv\İ!WAA_E[lA OwR xohxq= bh27ϭ+e,4A!tr.(hk'h*h5` emJ,(2;)JS k(Y_hP?>B\ B0 5ambBC5 n1qPܠ!8ԀJPyrC hhX o}.o8(C243&Ϛ:RYê 5'k+A`GXWpg :gX1ahvGh5zbdP֠WlQ+`>Zy94 rCv>7Xq(Uѧ8hhQb0+tк2CCNq 5b0w# 4K *! MvS 5p~{*wF^nY V7%$V)g7{RխXDlXTX(>=LJ +ZiI1h?58cBƈ!?אx&4S `F8'Pp! b;̆4D|g5[G5Ĭ*vb?rh jQ&kxo%ibxSʊܐ;4&9:4(Cqt߄nW_ Vp!M Cލr.FBU^.o Oo=JS}12A[;&fF&&rDŽ;A eh7QE]ia{be AhP4(k)+x7wb1(M8G-77@b>2A /q27|l,'1ee-ŷǣ?X1*pbkeh*&P} /5Ԏ6T円1iA RZP bЖ;(hH o ]C.04k!LhTD[İ|aBCOX' Ah`nNm 2, a VO&\nee)~Bܙe`h25@5PBGmh 8ĸ_\*@y*b:V?YkdP l Xa#6o *o ֍v!F -;T^3,D8Ԇl HK j:ze s o\z@v gr,\A_a1EOPwϏ&!U?3b! (C!@֐ b y ` AQjbV*([CF \srD{biXä' 6`r V5&7=ld^ sa s"۽Ern8>Ӣu=dC b1ĪƊ-t42Ϭ6 m=m<%rn4s5Z&>xVYGQ2WNp65045t{f@` >˫#ha ]GEJ蠞C&=š2cMŠCФBAC+M h8|A\jC7plÊH04C6 ՘2`M cb`hx"=֐3rȡĐ UڽN + ,1Y^hh}Z~w`c_Pа?pE b]l QB3~G퓇2։ߓH4w]z&EpWzKpCCqƯ;eoN^k&g7V)\7ޯp7M`>HUqK?֠N~z;ͲpGEN|QA3nr [U e8SzZ|; d)e45xT֐P`C./yRnPRašv4bqg c"@֕&bР ihq+44 5XkACoА M N +w?׼c?c` w@C>ݲ0!hHCPkPPbH+A4`$Cp=!_4(22!?MZ d$ 45(q`h~]^uz{0x"A h@!  L9`cӠ52;kUpCpZ [mie?Jrh:nT!␠&V́{#'[RhP`!g2 3p?X= 55aV?T*CCTVn 5ܻ + w}o%e4É/*^s53lǍٲu 4p'{.^TD ,7,0@+AH&xꄂ{7c (CWЀ8BCQP\>IeBUh2ĠA!gk,KWBe 7•_qeh`e@ὗ-8"!Kpb!ΰh,9rmJY7͝MX5Ć|ڂb@+*yT=Xkh1ڠauPtoE*C>$b & AeP`!<0/PjP[ C&4*@:u {-(CMWP0a)]Rv( gDW ` |˂3 ʐuZ'eRϫBC #+h52diPtIE_M>yqXRYv)Ā%]Рfd*he4p!4yf DU ]ekXB.Xn`㗭AuIE ^uŒ5/ 5tm <-/ .D~ /v5S9 50Ux<˪v.1HTm+4r&V2κeKZE g(M ֐Mo|D n@oXؼVCjG޷k ]D{3/gtHERnP1rE|؜2!gh(3eO-40?1Xho‰m@ӹP_Pe 6 r"ѐeH@CK"6P 2,S501(hwgg b`e`h"a|@AA%T 5ʭd?|Kc5p@d+enNdRh8udo[6_u* !6Uob;?͗k/h 8.$ b 5P0p= uqeRak[תe ]sp0/OäŁ+( b1YO^{cY{'Vsj(3T< Ӑj5$B aVĐ8É!oO wT}wǷwB m2CsT : *jddmx_Tw 5J<=J Kmܻ-篣u l v@ÚA(e@DWbt7!hXCݲF;o>TH g߾&k4D #*n܂% mEM,6fKbu綧Abʀ#٬ 5BC;:a3aDYCzG>qV7DLiY~=>{yxh`k!04k :!q^fb>b[Cmw_cqy}w'[ƠA hYlgh 2C׋2n]dT!xnBu(< `'RQ K/Yw^wQ,5̟ix_ kzubjL ;&4$!yN `eNʐVL>bg`k G<Ba8p+k45FCX',5R\WW~Y ,g'D;ʀ*0l\3921?MzZCZoGM@|tH\1k&TRq8àa !C Zg*CC ozdeP1k1{(\2G@ ! l P P rT 3VX#0H {u.xF4/}oT>01Xg`e42>. xNXk@'9\ ~>h4ȕ!hHC'X45< ]Ka)@1yij[0Pk kZu) k4:ٰ6HAqP`tdJ@2!=X3qeH@C8OP7kFtLC+XC~a84J8184ߪYs|PĚQ8@ Arde=ÿ|zGE Ah@! I`((X&_! ֠21ou ~s߄jP`C U P<-gPX=z3A"o9yS1 &NuE!6ѐ |[`tSWcex2 *րo띲pE巙 ߩXP]?ZaPְ2nC)99u2|C=k(`?B~D2xa8 P b0(bHMdpbphHݞeb]:NAai:÷é45XeB&l5Id21hP  @TD! Ņ~|'_?|L m>y( 4noz2y M Zbucmo׼g:r2h mG3.YPц 4$&\U)~eeZUq൪ZCwцV-nXa7JPi ܡQ ZC4E90uwcgA="]q'7iЀ$m450a uYQ>3&2401(hP'S_AA)P@C;?p}z{&/C>/{bP3 UP`ắNFe5^+IDATzA oq)4p.o띲X% #^7N48Ru8m5p֐Ёaۄۂ::A ~*k424@ ֋!m AoXhБZA8!a k5@Cba'VE5AAC2A)S4$O}Nޕ ͉MZaw Q̆ & А2$ by 0KVJA=Y@ޒOS!GP"ʀu_qWW|~@'25 PnETFCAPPXCH@\:9BAo r3L0ӹ{,  r /b0$ yHo=e, b @ !?]a8ְn.֐@;Ctx[ٮ;UU4MxO˄C 1h0(hh1PV'!&PlkY` ,~T{.eu,xϗ, <3On<XTD" P |ML8Ja z+ 㧠 ʀ}2+@Cl@Dpme'Ѹib{쩖o;migᅯ[gDbexש XPXkP@Xj@Q6D$wleA>+ZAX(e(ep6@ yhH[C0C"PO[-ay{V=*Coq?r&cZ 5Ad-4̖44t D=-C|{%Z3}% ḵ jkH(*C0@p!:qϏv; hʠqwɲPXBېoЀ b 8TX5045 .׼cCьla8_ &kAp2 `dĸ 2Ĭk5CC6e ͹aӻRtF`P+#*)Cqo~ldfD,4! ↶&m aoT  \:l.zV! Q vF:0BeNø)h`_0^ 9g Lh`e%T>&W}1v}yz'jЀTg0.Ĕp=}ٷ 4=OO6c?@Ž`:~"4AC+Рn&~xAV5|; ӯ;pۂ_ 5T3b=,ʹT6JV*40khmX7u>!@;T! CUU : 5(eȁyDݝZ[hN~o4 ܪtvǎZPϧ֠6<0 l!P-o`3245(ePրk?JDZkO r 4&:8y%uo3!/֐|dX 8jРz(b ˂wl`a9@׿Rv5XnPʡ9u1wA=Գ;45#-=0ij2grCаo"dpbphhS!vd^bt6 45c (&Q Ib Ae`kșȐH48j/$V}a4Ys/`F2#|2"dc$ 33͔2ܲEr͢@XC  },Ԑ4'\t߶1l \!&X 5Ġ}AA& ɍENeu@!p p=Eg^)谏'?v.Mb (%  ^ ikh=0`!Pj"=4*mza&ɿg{@Nh( 5A';& 1hw2ʐ;jAMky%94t U&* q;)8Ρ`eİq jL I@)Y˂ lR`g@2 $MT/(C.}d(WdXĠEN8m,4kNfk2P lBǶV~ =jC;#7pӄ%uC Nՙpr r L oР/,d2hNk d^ UnpkBn:! UXC h8D oА?2=rh-VUeh ZP)`I‰ۼb4v ԃ11(h ? 7KCJA wG(ؕXߋg4 P\l<sXa,oCh- D* \Ґ/=qhI4Xh1Txz8`C GHE h}g$ݽM|1S]" :bNbQ4)냭1e `Krh -FZ0GkfWw~w͢Cբ@C H(FAWz3צC=ehZ 8dB_N ,Mp7B+0`V l%flXaK Mdl Uh q;3xhC(h n2F,( 1k S_ACp]K Gu&oKfR2P`Q.Dv1 l D  :mX @ Р~hoA52x94@C%kh ZChCmXņ.JӞ/t:9+}[<2DUhP!l(y0ΐ1 P8C`!dཏWcfkX9풕a\,>f'2錀T+J(>=Ųsd,U@OaPbP#82(eݠ5 Qy(Xsp@PbȁN42dB 8XDQ e`hDC(,q *Ѡ_b~DX,4 44a%#ceY@,8r&NִhPGW ]XC+цl ʔx,ňF@֠VUi=q3` o=}mhbW?|>33 i8Wof l { 451gLU @R` xa DĔtL!h 8NW2\ʇqWhS4`K_  n­ hKJ b P5Xh7*=p复 p%AA1"ɉy1hXCG8ƟEAMw)|y94n a4X.,4>mR ,24N v&eGGczVC޼^gX\W>M@ (gRp /Y P \3e'^X,h&:{K-/|hd ͣ ]XC%nAAEnhYC%wg^ŪCAA^P 34A77;VGq eN "CC 4 dPxλq%vD&֭pz=6.5g ׬XAJ/Ġ%[Y1>4S2\V GɲNv((Ce+n3 @ .4%4AΆxtKqA!\ hos%a5\p6,gc8dA>;CAL4L}/W/24 cG5#bUż&к5T6{ /n,4zPiDzdg8R {ɚ:kYlDCeDŽN  C{}T gC&-F~[^2 45(eXzP5qG?ib"Cx"j`eA`['5`4o PcۯK䃇M045@ jjxXw}o Kű `ᶿߥ=[1\44p=x}*D  KC4'Xh4PЏ,V\(kNpC)4ȃyߵ+CC?$4T!ZoJ;3V=X80A68gu e`h P DraАAN  s G2{K7/(b`eX:`хm5tXk=>a B {7B)í)P:_ h24`2 (@C0(.{)h0Y9蛐QW<]X(,΃|}UCk`nP?GN04&Dd*`he"ks@ \fr][p @,XCx|C4XYPYC=n2; MXkPv0VzVRhTq( Tߊ;44:AuL0,~SoC Aʀ kPȽ?}o|"ԓb(3 y hDCJBY>6E >kvR5yK@ *(k ˳!Wkp^ ͡5Ԏ6td ц.b0X_u⽞'{9+oUr&J z gx%A5Mݡ c&ZQ@09FMd$[?İ ?޾yYSOɎa'_LAqOd)e#}sYq6OĘ P & )n`ePYge`h`k8@g`e`k`eh|6۾ b <[eȇh eP 'Aa(܂X jx*[C*+hAku0/C k c&,4Kb jOd:"A.> W>؈ 1`)ko3D4v;&kȇ3 ѐS5-5ȅCǻz(o[ICI(7S[h X}1BCG/p^x6N4 e4XS}F|>ǝ2XkPO^4ԶzцN{dB5tCJi !ܐg#×`ok h2T9 >K +04(/lApA(Vn*4؏D;O]ݬ|l44,n@` j#k20=^1h_ e @(ѧ~ kV Р3`[vg,2LM"lec`?kލ1YVu(! 82 :Rqk |l'#BG0|.44Н5F܁&z`hhmJ&P58Cʩ_jn6M̑2 pz~);m>Ch6Dg4{Rډ .3&AE #e.mn4.[82BV*Π&hZ-42R Q1/pġo*X(`roNpkSPt#E2%Ġ]Hg\X>N,='iyy%jDr@*hh7\c7ך~5ԋ<SdoYkfT+>`A!8r+CV55$AMF<*q_2LLdXjp"e`h !;K#AY6+ fR Kޮ⠞209ʠ!x)kȁ%oB` dgv1Bs \~qTh1JuՋL4 b 1gs.W_󺽼u˻7ZCChhn kkHpC;4$ 1AU_4@t:%zʠAQ.Je ݺ2Se:aАUd h౎c8- 7qh%y9`P֠B{$9N2(hP`C j+|.g*o(M4H> \3eBA)eH4zƦbw=3 ;nxɤiBvȲIVРQANjIh+5GlD02VܻM: rcb@3b407@,4`4F+bhk`nh jSkHGjܥ4#nha{cŊ}VH"ʃY85 V}mCl`&ʠk f͕hHG|AkPT{NpRڜ1Qϖ[eP֠x=<=71'JLMd PP1Rĸk{ו2D+ `ePP\ewH4>`h5  roޒ5 W*hc8Ѱ4u@uJlLp|w XeM (C`7R׏cWo/4hC+Њ5drCАw) =$+m]8E~@(kPj~ s P,zB U!g4UC>/(hpkphX-GN3 ]qaNFAD(!xh20ѶK2,491:! K804g  6€-.dBG@'e8EIOPp #e`ˇAF['0koT*aob{ζXh`nx <&8gstA 2 Lk[8NP7O|k +80kȏ6̋5gh7t RhihhN$aqa*C&7! &zV/8484FkObĠA /LuF%@ 1hDK.>Od{)eNYS3 2Xb,Kߚ3 4px$6GS>Wp(CBdA l  8rF:2gPE,1:=ggH(;,pj#Csf)42Ġߋ[ \2Zn4G 7߽ĠjAYC+P֖.~õvu =d+`! !S}k(CwP/kj~ 7qdr!h@bOvó2m[ē'i,% 34XnRʐxck f4n7X 5p>Ϗ}}(ñ-c[pieP֠>>1h`nۏ|4?]Fwl4/|F ?=`DQk wl嘆m akS7}WfX[p!3ڠ~-F2aZ웨 -AU5V+Oz7|z s 97T 14W;r_phphpk@C^I gb+) L *{ b8G! i-V kаƜCpc1,OĞÇ204'g@↥g `GoZh؋,4D  8NBXϨiBA ihH|*uRQh\sXԡr)|hP`An'\2A-8 -ZC)74ݼC=kH0}nuMaM*C~o5TR! 22 Z; ҭ!g w`ݴ[[nP { Ġ! jAxtaV '](+>E2ع Qy6q *[ JTX&Nn:24uSְM47\(C xBp U>W/vhì jdC=hȉ3TvšEgkP‶|nΟZ8+Ê0CnPq*ChV4< i!HβL8 2 `1RyJdپ 1h+ ( `A^^_P P\8YM(~6BO J`.^ >qҮ,̡?Od,C Nn !h8@9mxyEA.0Av(@$ q@MĠ/she(8JbЀܠ&8 /ElRU4/_}X@_5>wާ?zy AehEk(6 ֐342؀C.4H"z]e7T>VJOWk?|;sXb{I2g1(qA`k:qy. 4qh:K|lXB GXh@_AX,4XQhZ L5GlP/Ak~:`Q?}P-+'_lʠ *kn%u勲ҧNp3WAAZW/NyukqCom87`mfE޼ND M :RsCOj74 phX`Cx3C9pߏwh2v7 bbAC"qheCΰ\C?; g߶UV鏆 p7v v$F32̀%e8iin[/a C_Рu s|Ƅ} M4 how*h1#8Vh,bz(3 hAsnP\8E~oO=fb  lAb`hw {x<4 V;4A`A͏U2vI`À ZQ F ! |VCDQn,:,(An ,x+}x(١g@14Xea F{{IF qH$ʀ1J}nkԹ*2Jϣ\,N+A604/&eQ2pzaK'{ ^^ÇцF:~ JCC8C2a׭Y9"!7?4 Sf 6_t!sCCCCD /z:lo𤫵iA8 + ww & +v@JKAJCDue0ک~l97T 4ܬ@hxٛ4D V0ΐ(l'\ 2}BXG]2e}!`O/ j  kx*:bV)K5/PuEE 7JA)lg А =@Ƅ71Ch2̊fk 504f1d6r )hXz|Z܀S-E46+;xGP% e4Sqo;mQ&]lQ@A37^^JZ## PZ3q eh 33 jLggX%0/?70k 7#'V/4$r \kPj0GlPfr]hpwFO XOVmau>hpywwɊE]@Q я;al*qY} 4@ pU+kTHa9e 24]РS֠e8-8-'m4WmobbdpRiA204?c˫OhB ]CGjC0Ĕ 4(A p@C}+Xf s 3ᆙ[aP Nf!K߸nnq[_Qk ؼ^Ί 2&xCp(PkP'X ;12pąnY鏀(H4uE h}VDdAYCe@ nw' ` H4'_T0atcY',RWO2}]f |]nmd 1_`eР&z3)t7R PРh_efuCp   GZk^Hlޮb%Āš@ }Mvw8k4$vBò,f *hH`MdkX'oBq\mpބ=*JAJ?!Րc/ZA'nD!` Q'|hl*ddIW.*nR &XV1ynR (hH+C~^^}&>tI14kHBB 2`B)48u9Po#h V$7̤"f =jk_ ֐>!{&Y6>S SC̹JDŽq,+ ;d1iB)C  q Tz{O Nde!ObQA Eq *CN~7aA%pnuڠ P`7:oW-anPq,?W/@C?gRT'UjDx#A^ R&f mMt +FzXìAA[CCC懭Ak2AA/`KʠR31io0H`bP 'pCM3xy z;"?O4K@Xk(d*C4*C hgphHDа7n?ڐ5ϡPubk7]99@7 ʠ!1dext܌hPP\>҅bD4   P J]'^h X=6Ơ& *kL4dP?5r6_] v k)GZ`JC(beՓ,2xӄQ1nȱцLbPʐ h'Π4 4󾉗>j }B V>aP'7848484T?(=?  b G.ր-\8َ0?+1YPde4d+[(CÈ;ePs2pU_ F/$ P6֠s |ɕ CPp%snz h#<~榷y ie6Ѩ0pe,4)Ю2B24Thg*la˄EphV2 =Gr)[n ĕ5p @:xj#Ӭ5`*$37'ʐ8o҆  ~[xA\|e/|M[>ur'Pz ;gTi({G ׿|p#CX(9oebX3L~  ŠatHf\ц54ܷ|P2ԃLePqYMkh`&ȄVvukXU7 88484TH^|кw rׯAAG`PX545`Z!>ao띲 ,45 B42(hٹoB/T,|=NǷPؤSWFK sQC+u!XAhH+21If񥀆+~ŸkA ĐPΕˡ! Akb XTzP Maq} ʐ 4dBC2 Szޢ ;nX͕x"ZY6H `"'ha&f ZU p0unphphphh5LEԷm8ewp-ٛO(ZYc@h5G ,C# kI>9KJO}VYOQ G-"6! ܕ? 4|;'Pk*H 45@徭VX\xA>|b9'\7XhPe 9q.PE qJ$P -BohZD[7mg -rL'jP5ik:-` x{ژ5pbye~kek31hX>~ #4AY(YXo 4p@[(, ?GB Ah58t{T>l&S~iC;Ġ΂#<5{2欸PzPoTА 5AavР*C?7 Z֫]܀nx|bnm-CdwY5LFE,z{(k rYc0!8/`;K}  |kn؁cq0}0};}:l`AAGlqK:1xy d '&$>Ot AbgH@Cվ&0hf;hPbаhC֐    O#W;f X^k 45(nг ? ePP\r|F@# ijN,Ϳu Ve 4 8 l  h204Oo hउ\8E l Pe JSp12d>4(kn24HĐ!bʐt P 웨 䆹nphphph輙5l bT܀}Լ꤀2042R|cQ_ +qzb_ܬ {,~w  `=?" Kt l -H)h{UI64@l;&r DdR1ku_ hZAA khh 5&aM MԃWarCwц֭! 1hpkphphh AkHuq@'TqAYÑjwP_ތ܀g SFyf 42 546'|t].Kat>ٿ>X[v,5Ά=w7)?ۣF3` ŋ?|OzI1Vpg$?&@yfQT@C emDN ^^+fe +ʐX;J} ihgobUACLZW Y 8848484̦"x f ',+8 Σ?Y+\  M5Lh_XC䴈`xA]O$" e Ob5;*eP$]:B:`BdbT3g@Ɣ9)Po/撠H/xyhX'f` ' ieHCmeHCC8Ci6a2@C9W8d+qx[6]:lNkz.N6rR vƦ&#\ȉ53l \`5(h`k>rd"oO|Pe %&M|, ;Lp ɟXʐd!kvȏTg`h)@CP3Y&Yᕇ׊%kXӫ5 ΀R>Se7 m)CmhH+C&42#7t1rSh78484842`e/'1u,Dx?Y]nl6$o l  b J d6G~|@P:H( gP`]) @S@%BC34 f @C243$! ACq֡aOh(ΐ e hpkphphUcdhb ؋rE N0[.CC*I7JRM[[iePsbDž TACZq 0 kZwk2 R 4 ahpeX0_0,t kX h~{?l {&Yn5Lld KYCrYT@G3 TD臥O(f(jP+CP@؃?l!8Р P AH ڠ N_jP5ΐVDh`e4PRhho!42nhl _ 4wm(9RqdA #eh*k?Oζ#2`Wω j:+MCK'55/O)/<3t%,Abʠ&_*:j4`1K~lNXА.4􀉶׆zʐ ih`en@C)4(e(V ]@CB@+Ê[σthphph4:io>a#ATkh`k# [kG2(k |5}xfB0>nPU@o'1J,8Cl4 TY2a6k7[//@15aḾ2C+3 ORePP M\V7{Eס +58484 N`  q8=5XhxH;4a|eA͆]{MDŽ r)nvsנ5oSӳ!9Ƞ AeʀR2p BJC k]nZ~94tgX}Т2 |94xu8B6#g%TD4YU ` j2T勱y 'Aeˠ W k=ivhEp V2ʐU |l%zy944*4 Ah-0۾vya P,42nhk̤C/:4BmR'P ۽o*P\5{т:oB69؉Wfݘ?rġ!Ca5XeH ʀ JiKB(VbM37)7a971fF&S׏-25Ħ? ?2p%J//Rhhb 1thmx"АO ͡*j@T! v! Jf VVuhH+CUhpeX0kHpа*pԹ yiKяv # ._ H*B)4ePg[25`jfxAUm"$Ș,4(eƸ,xyyԌP f Р|! M o¡~NthphXn=XN"&d?LFa~GéT4WFb J2v i<12p],R?DK^^^;!ouehgH@CZTA)| hhWz熆цa&/ߒJ7@%fhYq SXJ} ra)P T s2 47"4g4*CU M/|94i Ikr\{W,8RGfz(axyyŝvwPLh247  4! - Ak`qHpڃ:=ҡk.m򟔗ʫk\, 3'PV&r -s@U4g-4Sh20454ׇ =[CCCo`]ha @44QNG3047"4(C)42(hFsC>PoʃC kVʠ.xyyyyyy)h`nhZ/r@CGP [mhh M&bаo-hh+瀆BC27xasC?ր1 PpkphrUepk%[v  +AiBDì ͡24*Cmh 1hYCׇyᆪ= VhCcz[\ʫ2,e=bH+C&4d*f4y:r"3аo" +o" ik\ Z B[CC5Wne7K+C4$o -:4$!*Y nh ^^^^^^^ -F5t ie9Ҳ2t =@C+q9e 4Tׇܐg p//////"754԰aop}r~¥///////^N XCP,q+Ka&}CАP 4gXy P:#'+qCskȁ >$ˡ4m(<A&")=$!VAeZCoq! Ahh1 4*&J5ԋ6`nX76Y84 SgW0/////U 97T TX`h4b)Z?Chh +o@C?А }BCkX ܰz! ! '{(\*vl:4PINo}Ch) v }hh2444!  _@+CCUhhfа&= 212CS_q 71Ch.PI4+Cmhhn 3R! UOth@Z'P:6@db]@( 8V  s mYsÊ9i! ah5yCCC}hJ3W *Reh 5IDUe5x?Ŋ >!o¡!` ^^^^^^ -BC)44WPCАP j@C eh = 7h pX1!  uˡukhqpC J:V aeMr_yc +C,7!e ^^^^^^ AC[ц4(Ce 5ahHCC242`xCܰ!raAÄdˡa&>L(eACqž82BC5憎7sphphpe 44&Ж20hH(&@CAACmnw<   Ք!V ˡ;nhR4XeAC=e KAAC24YMvI[CmnV>GPTpP,4 Vb0  3ԀjBjpCܰ!h P//////^=|S54T 2 @zļCC+ĐR` >aM'V45ĸhzD|ehH0a8}JPZ'k~+`$ޡk ц>!gpCC8C2(bjz2(h(U8@C%eo?P|phph CC0?4 +CЖ2t =O4`3&ĀE!m г587̝,8484xyyyyyyyeA6 h24ze`hT&PUJ! ^sĆ~5T7 SʷUk%WP/a Z/,SjaAà&j@C eh2! AVܐc 9s"gb pZp嵒627T4Pzʐ iehoaqhhr" ikn0Ckpn(.ԡK! r!H 1e0w7!PoF!sC&4plaesì]@sCCCætRE,4

#e'Kt7`6mE-u[ 0c9!FLRUpYg5K1Ȃ%YOeA mFrwNgA`Εff25qP*( 9p-8 &\˽y^%SCͤA āxh4mF I/[v} NVlX=CToI`:)ݍ>Iҍ{+xFXŤ꟨!fK# /VKWJ#  tO]\L^qϢE 'xkt ncN!"HS3FZQonXi:8A<ğt^*쎵n0Qϵ'ӚK<(r V *{`0(\>CPK!Xªppt/presProps.xml;0 @w$e) U,n)q";nOGe纻{'@l6r]R[y>V;)8i |ˮ].XEL:eH"+)X)f0@^<Ҩzҷ|;)˭ڢ4` 샹W O6~KRPK!spcdocProps/app.xml (To0~S+ BuW]Ť"vϮs!;=Z7)sFϢ?ha27>M"<WF,ځOteMKp)E4(䮏iؒ{t&6y.`póA֫:(0NI3#C`FSJe ,J726sl88ܣ"l)55y2R{QpTm3=',&lߖc#tKR;>Cw9^?"H+u>wRt >Ii2PDGnydUKbrQrs~^6t 9ka$!8dxN=GskA\rMNpQN6WPOcv7MYqcoz#5ũBkA@HWa: u-d[.pIZD¢fɰ?YAm,JfPK![wodocProps/core.xml (QK0C{YBہ@-&%ߛn(s{oű+E(ŵjr()* GX(2P <݀qlIRh\C1|53P^hS3f?pEs\c9{`LD|B6{S 1TPr?^ ʙk|Oqق>Z9۶'u4Jϊ*2bcP|7t 0MQ2v7N+fe#Avj_` $?8Lb|D{$NK+W#MK($~]!߿42&3 P /WPK-!m5@ [Content_Types].xmlPK-!ht y_rels/.relsPK-!4OE ppt/slides/_rels/slide4.xml.relsPK-!  ppt/slides/_rels/slide3.xml.relsPK-!K=7 ppt/slides/_rels/slide2.xml.relsPK-!c\#7 ppt/slides/_rels/slide1.xml.relsPK-!?X ppt/slides/_rels/slide5.xml.relsPK-!!E ppt/slides/_rels/slide6.xml.relsPK-![ ppt/slides/_rels/slide7.xml.relsPK-!BE 3ppt/slides/_rels/slide8.xml.relsPK-!B{ Tppt/slides/_rels/slide9.xml.relsPK-! jG!ppt/slides/_rels/slide10.xml.relsPK-!hG\ppt/_rels/presentation.xml.relsPK-!." Dppt/presentation.xmlPK-!L0Lppt/slides/slide2.xmlPK-!` y&Zppt/slides/slide3.xmlPK-!Hw ppt/slides/slide4.xmlPK-!O7%ppt/slides/slide5.xmlPK-!E/v(*ppt/slides/slide10.xmlPK-!YaM,1ppt/slides/slide6.xmlPK-!|Ƣ8{8ppt/slides/slide8.xmlPK-!kP@ppt/slides/slide9.xmlPK-!|VEppt/slides/slide1.xmlPK-!MGppt/slides/slide7.xmlPK-!ђ7,Lppt/slideLayouts/_rels/slideLayout1.xml.relsPK-!ђ7- Mppt/slideLayouts/_rels/slideLayout11.xml.relsPK-!ђ7,Nppt/slideLayouts/_rels/slideLayout9.xml.relsPK-!ђ7,Oppt/slideLayouts/_rels/slideLayout8.xml.relsPK-!i_!,#Pppt/slideMasters/_rels/slideMaster1.xml.relsPK-!ђ7,Qppt/slideLayouts/_rels/slideLayout3.xml.relsPK-!ђ7,Rppt/slideLayouts/_rels/slideLayout4.xml.relsPK-!ђ7,Sppt/slideLayouts/_rels/slideLayout5.xml.relsPK-!ђ7,Tppt/slideLayouts/_rels/slideLayout6.xml.relsPK-! 1/!Uppt/slideMasters/slideMaster1.xmlPK-!ђ7,{]ppt/slideLayouts/_rels/slideLayout7.xml.relsPK-!ђ7-^ppt/slideLayouts/_rels/slideLayout10.xml.relsPK-!ђ7,_ppt/slideLayouts/_rels/slideLayout2.xml.relsPK-!;:իa' "`ppt/slideLayouts/slideLayout10.xmlPK-!.5_!5dppt/slideLayouts/slideLayout1.xmlPK-!*1J !hppt/slideLayouts/slideLayout2.xmlPK-!Jj!2lppt/slideLayouts/slideLayout3.xmlPK-!R;!pppt/slideLayouts/slideLayout4.xmlPK-!nn!Fuppt/slideLayouts/slideLayout5.xmlPK-!Ϙ!zppt/slideLayouts/slideLayout6.xmlPK-!]P!~ppt/slideLayouts/slideLayout9.xmlPK-!ǵ7!ppt/slideLayouts/slideLayout8.xmlPK-!m!ppt/slideLayouts/slideLayout7.xmlPK-!J]ڱ "ppt/slideLayouts/slideLayout11.xmlPK-! 9\ppt/theme/theme1.xmlPK- !zdocProps/thumbnail.jpegPK- ! ܱppt/media/image3.pngPK- !*++ppt/media/image8.pngPK- !iǽlNppt/media/image7.pngPK- !>/%[ppt/media/image6.pngPK- !ٳVppt/media/image5.pngPK- !&66tppt/media/image4.pngPK- !tBppt/media/image9.pngPK- ! Pppppt/media/image10.pngPK- !Gddxppt/media/image11.pngPK- !졿))7 ppt/media/image1.pngPK- !`ppt/media/image2.pngPK- !m88 ppt/media/image14.pngPK- !NqÁÁ ppt/media/image12.pngPK- !p:  ppt/media/image13.pngPK-! ppt/tableStyles.xmlPK-!TEH ppt/viewProps.xmlPK-!Xªl ppt/presProps.xmlPK-!spcE docProps/app.xmlPK-![wo docProps/core.xmlPKEE  fcl-0.5.0/test/fcl_resources/scenario-1-2-3/Model_1_Scenario_1.txt000066400000000000000000014722611274356571000244500ustar00rootroot00000000000000 fcl-0.5.0/test/fcl_resources/scenario-1-2-3/Model_1_Scenario_2.txt000066400000000000000000014722621274356571000244520ustar00rootroot00000000000000 fcl-0.5.0/test/fcl_resources/scenario-1-2-3/Model_1_Scenario_3.txt000066400000000000000000013211071274356571000244420ustar00rootroot00000000000000 fcl-0.5.0/test/fcl_resources/scenario-1-2-3/Model_2_Scenario_1.txt000066400000000000000000006177221274356571000244530ustar00rootroot00000000000000 fcl-0.5.0/test/fcl_resources/scenario-1-2-3/Model_2_Scenario_2.txt000066400000000000000000006177201274356571000244520ustar00rootroot00000000000000 fcl-0.5.0/test/fcl_resources/scenario-1-2-3/Model_2_Scenario_3.txt000066400000000000000000061160671274356571000244570ustar00rootroot00000000000000 fcl-0.5.0/test/fcl_resources/scenario-1-2-3/Model_3_Scenario_1.txt000066400000000000000000032007251274356571000244460ustar00rootroot00000000000000 fcl-0.5.0/test/fcl_resources/scenario-1-2-3/Model_3_Scenario_2.txt000066400000000000000000032007241274356571000244460ustar00rootroot00000000000000 fcl-0.5.0/test/fcl_resources/scenario-1-2-3/Model_3_Scenario_3.txt000066400000000000000000264276501274356571000244650ustar00rootroot00000000000000 fcl-0.5.0/test/general_test.cpp000066400000000000000000000030171274356571000164320ustar00rootroot00000000000000#include #include #include #include #include using namespace std; using namespace fcl; int main(int argc, char** argv) { std::shared_ptr box0(new Box(1,1,1)); std::shared_ptr box1(new Box(1,1,1)); // GJKSolver_indep solver; GJKSolver_libccd solver; Vec3f contact_points; FCL_REAL penetration_depth; Vec3f normal; Transform3f tf0, tf1; tf0.setIdentity(); tf0.setTranslation(Vec3f(.9,0,0)); tf0.setQuatRotation(Quaternion3f(.6, .8, 0, 0)); tf1.setIdentity(); bool res = solver.shapeIntersect(*box0, tf0, *box1, tf1, &contact_points, &penetration_depth, &normal); cout << "contact points: " << contact_points << endl; cout << "pen depth: " << penetration_depth << endl; cout << "normal: " << normal << endl; cout << "result: " << res << endl; static const int num_max_contacts = std::numeric_limits::max(); static const bool enable_contact = true; fcl::CollisionResult result; fcl::CollisionRequest request(num_max_contacts, enable_contact); CollisionObject co0(box0, tf0); CollisionObject co1(box1, tf1); fcl::collide(&co0, &co1, request, result); vector contacts; result.getContacts(contacts); cout << contacts.size() << " contacts found" << endl; for(const Contact &contact : contacts) { cout << "position: " << contact.pos << endl; } }fcl-0.5.0/test/libsvm/000077500000000000000000000000001274356571000145455ustar00rootroot00000000000000fcl-0.5.0/test/libsvm/svm.cpp000066400000000000000000002171051274356571000160640ustar00rootroot00000000000000#include #include #include #include #include #include #include #include #include #include #include "svm.h" int libsvm_version = LIBSVM_VERSION; typedef float Qfloat; typedef signed char schar; #ifndef min template static inline T min(T x,T y) { return (x static inline T max(T x,T y) { return (x>y)?x:y; } #endif template static inline void swap(T& x, T& y) { T t=x; x=y; y=t; } template static inline void clone(T*& dst, S* src, int n) { dst = new T[n]; memcpy((void *)dst,(void *)src,sizeof(T)*n); } static inline double powi(double base, int times) { double tmp = base, ret = 1.0; for(int t=times; t>0; t/=2) { if(t%2==1) ret*=tmp; tmp = tmp * tmp; } return ret; } #define INF HUGE_VAL #define TAU 1e-12 #define Malloc(type,n) (type *)malloc((n)*sizeof(type)) static void print_string_stdout(const char *s) { fputs(s,stdout); fflush(stdout); } static void (*svm_print_string) (const char *) = &print_string_stdout; #if 0 static void info(const char *fmt,...) { char buf[BUFSIZ]; va_list ap; va_start(ap,fmt); vsprintf(buf,fmt,ap); va_end(ap); (*svm_print_string)(buf); } #else static void info(const char *fmt,...) {} #endif // // Kernel Cache // // l is the number of total data items // size is the cache size limit in bytes // class Cache { public: Cache(int l,long int size); ~Cache(); // request data [0,len) // return some position p where [p,len) need to be filled // (p >= len if nothing needs to be filled) int get_data(const int index, Qfloat **data, int len); void swap_index(int i, int j); private: int l; long int size; struct head_t { head_t *prev, *next; // a circular list Qfloat *data; int len; // data[0,len) is cached in this entry }; head_t *head; head_t lru_head; void lru_delete(head_t *h); void lru_insert(head_t *h); }; Cache::Cache(int l_,long int size_):l(l_),size(size_) { head = (head_t *)calloc(l,sizeof(head_t)); // initialized to 0 size /= sizeof(Qfloat); size -= l * sizeof(head_t) / sizeof(Qfloat); size = max(size, 2 * (long int) l); // cache must be large enough for two columns lru_head.next = lru_head.prev = &lru_head; } Cache::~Cache() { for(head_t *h = lru_head.next; h != &lru_head; h=h->next) free(h->data); free(head); } void Cache::lru_delete(head_t *h) { // delete from current location h->prev->next = h->next; h->next->prev = h->prev; } void Cache::lru_insert(head_t *h) { // insert to last position h->next = &lru_head; h->prev = lru_head.prev; h->prev->next = h; h->next->prev = h; } int Cache::get_data(const int index, Qfloat **data, int len) { head_t *h = &head[index]; if(h->len) lru_delete(h); int more = len - h->len; if(more > 0) { // free old space while(size < more) { head_t *old = lru_head.next; lru_delete(old); free(old->data); size += old->len; old->data = 0; old->len = 0; } // allocate new space h->data = (Qfloat *)realloc(h->data,sizeof(Qfloat)*len); size -= more; swap(h->len,len); } lru_insert(h); *data = h->data; return len; } void Cache::swap_index(int i, int j) { if(i==j) return; if(head[i].len) lru_delete(&head[i]); if(head[j].len) lru_delete(&head[j]); swap(head[i].data,head[j].data); swap(head[i].len,head[j].len); if(head[i].len) lru_insert(&head[i]); if(head[j].len) lru_insert(&head[j]); if(i>j) swap(i,j); for(head_t *h = lru_head.next; h!=&lru_head; h=h->next) { if(h->len > i) { if(h->len > j) swap(h->data[i],h->data[j]); else { // give up lru_delete(h); free(h->data); size += h->len; h->data = 0; h->len = 0; } } } } // // Kernel evaluation // // the static method k_function is for doing single kernel evaluation // the constructor of Kernel prepares to calculate the l*l kernel matrix // the member function get_Q is for getting one column from the Q Matrix // class QMatrix { public: virtual Qfloat *get_Q(int column, int len) const = 0; virtual double *get_QD() const = 0; virtual void swap_index(int i, int j) const = 0; virtual ~QMatrix() {} }; class Kernel: public QMatrix { public: Kernel(int l, svm_node * const * x, const svm_parameter& param); virtual ~Kernel(); static double k_function(const svm_node *x, const svm_node *y, const svm_parameter& param); virtual Qfloat *get_Q(int column, int len) const = 0; virtual double *get_QD() const = 0; virtual void swap_index(int i, int j) const // no so const... { swap(x[i],x[j]); if(x_square) swap(x_square[i],x_square[j]); } protected: double (Kernel::*kernel_function)(int i, int j) const; private: const svm_node **x; double *x_square; // svm_parameter const int kernel_type; const int degree; const double gamma; const double coef0; static double dot(const svm_node *px, const svm_node *py); double kernel_linear(int i, int j) const { return dot(x[i],x[j]); } double kernel_poly(int i, int j) const { return powi(gamma*dot(x[i],x[j])+coef0,degree); } double kernel_rbf(int i, int j) const { return exp(-gamma*(x_square[i]+x_square[j]-2*dot(x[i],x[j]))); } double kernel_sigmoid(int i, int j) const { return tanh(gamma*dot(x[i],x[j])+coef0); } double kernel_precomputed(int i, int j) const { return x[i][(int)(x[j][0].value)].value; } }; double k_function(const svm_node* x, const svm_node* y, const svm_parameter& param) { return Kernel::k_function(x, y, param); } Kernel::Kernel(int l, svm_node * const * x_, const svm_parameter& param) :kernel_type(param.kernel_type), degree(param.degree), gamma(param.gamma), coef0(param.coef0) { switch(kernel_type) { case LINEAR: kernel_function = &Kernel::kernel_linear; break; case POLY: kernel_function = &Kernel::kernel_poly; break; case RBF: kernel_function = &Kernel::kernel_rbf; break; case SIGMOID: kernel_function = &Kernel::kernel_sigmoid; break; case PRECOMPUTED: kernel_function = &Kernel::kernel_precomputed; break; } clone(x,x_,l); if(kernel_type == RBF) { x_square = new double[l]; for(int i=0;iindex != -1 && py->index != -1) { if(px->index == py->index) { sum += px->value * py->value; ++px; ++py; } else { if(px->index > py->index) ++py; else ++px; } } return sum; } double Kernel::k_function(const svm_node *x, const svm_node *y, const svm_parameter& param) { switch(param.kernel_type) { case LINEAR: return dot(x,y); case POLY: return powi(param.gamma*dot(x,y)+param.coef0,param.degree); case RBF: { double sum = 0; while(x->index != -1 && y->index !=-1) { if(x->index == y->index) { double d = x->value - y->value; sum += d*d; ++x; ++y; } else { if(x->index > y->index) { sum += y->value * y->value; ++y; } else { sum += x->value * x->value; ++x; } } } while(x->index != -1) { sum += x->value * x->value; ++x; } while(y->index != -1) { sum += y->value * y->value; ++y; } return exp(-param.gamma*sum); } case SIGMOID: return tanh(param.gamma*dot(x,y)+param.coef0); case PRECOMPUTED: //x: test (validation), y: SV return x[(int)(y->value)].value; default: return 0; // Unreachable } } // An SMO algorithm in Fan et al., JMLR 6(2005), p. 1889--1918 // Solves: // // min 0.5(\alpha^T Q \alpha) + p^T \alpha // // y^T \alpha = \delta // y_i = +1 or -1 // 0 <= alpha_i <= Cp for y_i = 1 // 0 <= alpha_i <= Cn for y_i = -1 // // Given: // // Q, p, y, Cp, Cn, and an initial feasible point \alpha // l is the size of vectors and matrices // eps is the stopping tolerance // // solution will be put in \alpha, objective value will be put in obj // class Solver { public: Solver() {}; virtual ~Solver() {}; struct SolutionInfo { double obj; double rho; double *upper_bound; double r; // for Solver_NU }; void Solve(int l, const QMatrix& Q, const double *p_, const schar *y_, double *alpha_, const double* C_, double eps, SolutionInfo* si, int shrinking); protected: int active_size; schar *y; double *G; // gradient of objective function enum { LOWER_BOUND, UPPER_BOUND, FREE }; char *alpha_status; // LOWER_BOUND, UPPER_BOUND, FREE double *alpha; const QMatrix *Q; const double *QD; double eps; double Cp,Cn; double *C; double *p; int *active_set; double *G_bar; // gradient, if we treat free variables as 0 int l; bool unshrink; // XXX double get_C(int i) { return C[i]; } void update_alpha_status(int i) { if(alpha[i] >= get_C(i)) alpha_status[i] = UPPER_BOUND; else if(alpha[i] <= 0) alpha_status[i] = LOWER_BOUND; else alpha_status[i] = FREE; } bool is_upper_bound(int i) { return alpha_status[i] == UPPER_BOUND; } bool is_lower_bound(int i) { return alpha_status[i] == LOWER_BOUND; } bool is_free(int i) { return alpha_status[i] == FREE; } void swap_index(int i, int j); void reconstruct_gradient(); virtual int select_working_set(int &i, int &j); virtual double calculate_rho(); virtual void do_shrinking(); private: bool be_shrunk(int i, double Gmax1, double Gmax2); }; void Solver::swap_index(int i, int j) { Q->swap_index(i,j); swap(y[i],y[j]); swap(G[i],G[j]); swap(alpha_status[i],alpha_status[j]); swap(alpha[i],alpha[j]); swap(p[i],p[j]); swap(active_set[i],active_set[j]); swap(G_bar[i],G_bar[j]); swap(C[i],C[j]); } void Solver::reconstruct_gradient() { // reconstruct inactive elements of G from G_bar and free variables if(active_size == l) return; int i,j; int nr_free = 0; for(j=active_size;j 2*active_size*(l-active_size)) { for(i=active_size;iget_Q(i,active_size); for(j=0;jget_Q(i,l); double alpha_i = alpha[i]; for(j=active_size;jl = l; this->Q = &Q; QD=Q.get_QD(); clone(p, p_,l); clone(y, y_,l); clone(alpha,alpha_,l); clone(C,C_,l); this->eps = eps; unshrink = false; // initialize alpha_status { alpha_status = new char[l]; for(int i=0;iINT_MAX/100 ? INT_MAX : 100*l); int counter = min(l,1000)+1; while(iter < max_iter) { // show progress and do shrinking if(--counter == 0) { counter = min(l,1000); if(shrinking) do_shrinking(); info("."); } int i,j; if(select_working_set(i,j)!=0) { // reconstruct the whole gradient reconstruct_gradient(); // reset active set size and check active_size = l; info("*"); if(select_working_set(i,j)!=0) break; else counter = 1; // do shrinking next iteration } ++iter; // update alpha[i] and alpha[j], handle bounds carefully const Qfloat *Q_i = Q.get_Q(i,active_size); const Qfloat *Q_j = Q.get_Q(j,active_size); double C_i = get_C(i); double C_j = get_C(j); double old_alpha_i = alpha[i]; double old_alpha_j = alpha[j]; if(y[i]!=y[j]) { double quad_coef = QD[i]+QD[j]+2*Q_i[j]; if (quad_coef <= 0) quad_coef = TAU; double delta = (-G[i]-G[j])/quad_coef; double diff = alpha[i] - alpha[j]; alpha[i] += delta; alpha[j] += delta; if(diff > 0) { if(alpha[j] < 0) { alpha[j] = 0; alpha[i] = diff; } } else { if(alpha[i] < 0) { alpha[i] = 0; alpha[j] = -diff; } } if(diff > C_i - C_j) { if(alpha[i] > C_i) { alpha[i] = C_i; alpha[j] = C_i - diff; } } else { if(alpha[j] > C_j) { alpha[j] = C_j; alpha[i] = C_j + diff; } } } else { double quad_coef = QD[i]+QD[j]-2*Q_i[j]; if (quad_coef <= 0) quad_coef = TAU; double delta = (G[i]-G[j])/quad_coef; double sum = alpha[i] + alpha[j]; alpha[i] -= delta; alpha[j] += delta; if(sum > C_i) { if(alpha[i] > C_i) { alpha[i] = C_i; alpha[j] = sum - C_i; } } else { if(alpha[j] < 0) { alpha[j] = 0; alpha[i] = sum; } } if(sum > C_j) { if(alpha[j] > C_j) { alpha[j] = C_j; alpha[i] = sum - C_j; } } else { if(alpha[i] < 0) { alpha[i] = 0; alpha[j] = sum; } } } // update G double delta_alpha_i = alpha[i] - old_alpha_i; double delta_alpha_j = alpha[j] - old_alpha_j; for(int k=0;k= max_iter) { if(active_size < l) { // reconstruct the whole gradient to calculate objective value reconstruct_gradient(); active_size = l; info("*"); } fprintf(stderr,"\nWARNING: reaching max number of iterations\n"); } // calculate rho si->rho = calculate_rho(); // calculate objective value { double v = 0; int i; for(i=0;iobj = v/2; } // put back the solution { for(int i=0;iupper_bound[i] = C[i]; info("\noptimization finished, #iter = %d\n",iter); delete[] p; delete[] y; delete[] C; delete[] alpha; delete[] alpha_status; delete[] active_set; delete[] G; delete[] G_bar; } // return 1 if already optimal, return 0 otherwise int Solver::select_working_set(int &out_i, int &out_j) { // return i,j such that // i: maximizes -y_i * grad(f)_i, i in I_up(\alpha) // j: minimizes the decrease of obj value // (if quadratic coefficeint <= 0, replace it with tau) // -y_j*grad(f)_j < -y_i*grad(f)_i, j in I_low(\alpha) double Gmax = -INF; double Gmax2 = -INF; int Gmax_idx = -1; int Gmin_idx = -1; double obj_diff_min = INF; for(int t=0;t= Gmax) { Gmax = -G[t]; Gmax_idx = t; } } else { if(!is_lower_bound(t)) if(G[t] >= Gmax) { Gmax = G[t]; Gmax_idx = t; } } int i = Gmax_idx; const Qfloat *Q_i = NULL; if(i != -1) // NULL Q_i not accessed: Gmax=-INF if i=-1 Q_i = Q->get_Q(i,active_size); for(int j=0;j= Gmax2) Gmax2 = G[j]; if (grad_diff > 0) { double obj_diff; double quad_coef = QD[i]+QD[j]-2.0*y[i]*Q_i[j]; if (quad_coef > 0) obj_diff = -(grad_diff*grad_diff)/quad_coef; else obj_diff = -(grad_diff*grad_diff)/TAU; if (obj_diff <= obj_diff_min) { Gmin_idx=j; obj_diff_min = obj_diff; } } } } else { if (!is_upper_bound(j)) { double grad_diff= Gmax-G[j]; if (-G[j] >= Gmax2) Gmax2 = -G[j]; if (grad_diff > 0) { double obj_diff; double quad_coef = QD[i]+QD[j]+2.0*y[i]*Q_i[j]; if (quad_coef > 0) obj_diff = -(grad_diff*grad_diff)/quad_coef; else obj_diff = -(grad_diff*grad_diff)/TAU; if (obj_diff <= obj_diff_min) { Gmin_idx=j; obj_diff_min = obj_diff; } } } } } if(Gmax+Gmax2 < eps) return 1; out_i = Gmax_idx; out_j = Gmin_idx; return 0; } bool Solver::be_shrunk(int i, double Gmax1, double Gmax2) { if(is_upper_bound(i)) { if(y[i]==+1) return(-G[i] > Gmax1); else return(-G[i] > Gmax2); } else if(is_lower_bound(i)) { if(y[i]==+1) return(G[i] > Gmax2); else return(G[i] > Gmax1); } else return(false); } void Solver::do_shrinking() { int i; double Gmax1 = -INF; // max { -y_i * grad(f)_i | i in I_up(\alpha) } double Gmax2 = -INF; // max { y_i * grad(f)_i | i in I_low(\alpha) } // find maximal violating pair first for(i=0;i= Gmax1) Gmax1 = -G[i]; } if(!is_lower_bound(i)) { if(G[i] >= Gmax2) Gmax2 = G[i]; } } else { if(!is_upper_bound(i)) { if(-G[i] >= Gmax2) Gmax2 = -G[i]; } if(!is_lower_bound(i)) { if(G[i] >= Gmax1) Gmax1 = G[i]; } } } if(unshrink == false && Gmax1 + Gmax2 <= eps*10) { unshrink = true; reconstruct_gradient(); active_size = l; info("*"); } for(i=0;i i) { if (!be_shrunk(active_size, Gmax1, Gmax2)) { swap_index(i,active_size); break; } active_size--; } } } double Solver::calculate_rho() { double r; int nr_free = 0; double ub = INF, lb = -INF, sum_free = 0; for(int i=0;i0) r = sum_free/nr_free; else r = (ub+lb)/2; return r; } // // Solver for nu-svm classification and regression // // additional constraint: e^T \alpha = constant // class Solver_NU : public Solver { public: Solver_NU() {} void Solve(int l, const QMatrix& Q, const double *p, const schar *y, double *alpha, double* C_, double eps, SolutionInfo* si, int shrinking) { this->si = si; Solver::Solve(l,Q,p,y,alpha,C_,eps,si,shrinking); } private: SolutionInfo *si; int select_working_set(int &i, int &j); double calculate_rho(); bool be_shrunk(int i, double Gmax1, double Gmax2, double Gmax3, double Gmax4); void do_shrinking(); }; // return 1 if already optimal, return 0 otherwise int Solver_NU::select_working_set(int &out_i, int &out_j) { // return i,j such that y_i = y_j and // i: maximizes -y_i * grad(f)_i, i in I_up(\alpha) // j: minimizes the decrease of obj value // (if quadratic coefficeint <= 0, replace it with tau) // -y_j*grad(f)_j < -y_i*grad(f)_i, j in I_low(\alpha) double Gmaxp = -INF; double Gmaxp2 = -INF; int Gmaxp_idx = -1; double Gmaxn = -INF; double Gmaxn2 = -INF; int Gmaxn_idx = -1; int Gmin_idx = -1; double obj_diff_min = INF; for(int t=0;t= Gmaxp) { Gmaxp = -G[t]; Gmaxp_idx = t; } } else { if(!is_lower_bound(t)) if(G[t] >= Gmaxn) { Gmaxn = G[t]; Gmaxn_idx = t; } } int ip = Gmaxp_idx; int in = Gmaxn_idx; const Qfloat *Q_ip = NULL; const Qfloat *Q_in = NULL; if(ip != -1) // NULL Q_ip not accessed: Gmaxp=-INF if ip=-1 Q_ip = Q->get_Q(ip,active_size); if(in != -1) Q_in = Q->get_Q(in,active_size); for(int j=0;j= Gmaxp2) Gmaxp2 = G[j]; if (grad_diff > 0) { double obj_diff; double quad_coef = QD[ip]+QD[j]-2*Q_ip[j]; if (quad_coef > 0) obj_diff = -(grad_diff*grad_diff)/quad_coef; else obj_diff = -(grad_diff*grad_diff)/TAU; if (obj_diff <= obj_diff_min) { Gmin_idx=j; obj_diff_min = obj_diff; } } } } else { if (!is_upper_bound(j)) { double grad_diff=Gmaxn-G[j]; if (-G[j] >= Gmaxn2) Gmaxn2 = -G[j]; if (grad_diff > 0) { double obj_diff; double quad_coef = QD[in]+QD[j]-2*Q_in[j]; if (quad_coef > 0) obj_diff = -(grad_diff*grad_diff)/quad_coef; else obj_diff = -(grad_diff*grad_diff)/TAU; if (obj_diff <= obj_diff_min) { Gmin_idx=j; obj_diff_min = obj_diff; } } } } } if(max(Gmaxp+Gmaxp2,Gmaxn+Gmaxn2) < eps) return 1; if (y[Gmin_idx] == +1) out_i = Gmaxp_idx; else out_i = Gmaxn_idx; out_j = Gmin_idx; return 0; } bool Solver_NU::be_shrunk(int i, double Gmax1, double Gmax2, double Gmax3, double Gmax4) { if(is_upper_bound(i)) { if(y[i]==+1) return(-G[i] > Gmax1); else return(-G[i] > Gmax4); } else if(is_lower_bound(i)) { if(y[i]==+1) return(G[i] > Gmax2); else return(G[i] > Gmax3); } else return(false); } void Solver_NU::do_shrinking() { double Gmax1 = -INF; // max { -y_i * grad(f)_i | y_i = +1, i in I_up(\alpha) } double Gmax2 = -INF; // max { y_i * grad(f)_i | y_i = +1, i in I_low(\alpha) } double Gmax3 = -INF; // max { -y_i * grad(f)_i | y_i = -1, i in I_up(\alpha) } double Gmax4 = -INF; // max { y_i * grad(f)_i | y_i = -1, i in I_low(\alpha) } // find maximal violating pair first int i; for(i=0;i Gmax1) Gmax1 = -G[i]; } else if(-G[i] > Gmax4) Gmax4 = -G[i]; } if(!is_lower_bound(i)) { if(y[i]==+1) { if(G[i] > Gmax2) Gmax2 = G[i]; } else if(G[i] > Gmax3) Gmax3 = G[i]; } } if(unshrink == false && max(Gmax1+Gmax2,Gmax3+Gmax4) <= eps*10) { unshrink = true; reconstruct_gradient(); active_size = l; } for(i=0;i i) { if (!be_shrunk(active_size, Gmax1, Gmax2, Gmax3, Gmax4)) { swap_index(i,active_size); break; } active_size--; } } } double Solver_NU::calculate_rho() { int nr_free1 = 0,nr_free2 = 0; double ub1 = INF, ub2 = INF; double lb1 = -INF, lb2 = -INF; double sum_free1 = 0, sum_free2 = 0; for(int i=0;i 0) r1 = sum_free1/nr_free1; else r1 = (ub1+lb1)/2; if(nr_free2 > 0) r2 = sum_free2/nr_free2; else r2 = (ub2+lb2)/2; si->r = (r1+r2)/2; return (r1-r2)/2; } // // Q matrices for various formulations // class SVC_Q: public Kernel { public: SVC_Q(const svm_problem& prob, const svm_parameter& param, const schar *y_) :Kernel(prob.l, prob.x, param) { clone(y,y_,prob.l); cache = new Cache(prob.l,(long int)(param.cache_size*(1<<20))); QD = new double[prob.l]; for(int i=0;i*kernel_function)(i,i); } Qfloat *get_Q(int i, int len) const { Qfloat *data; int start, j; if((start = cache->get_data(i,&data,len)) < len) { for(j=start;j*kernel_function)(i,j)); } return data; } double *get_QD() const { return QD; } void swap_index(int i, int j) const { cache->swap_index(i,j); Kernel::swap_index(i,j); swap(y[i],y[j]); swap(QD[i],QD[j]); } ~SVC_Q() { delete[] y; delete cache; delete[] QD; } private: schar *y; Cache *cache; double *QD; }; class ONE_CLASS_Q: public Kernel { public: ONE_CLASS_Q(const svm_problem& prob, const svm_parameter& param) :Kernel(prob.l, prob.x, param) { cache = new Cache(prob.l,(long int)(param.cache_size*(1<<20))); QD = new double[prob.l]; for(int i=0;i*kernel_function)(i,i); } Qfloat *get_Q(int i, int len) const { Qfloat *data; int start, j; if((start = cache->get_data(i,&data,len)) < len) { for(j=start;j*kernel_function)(i,j); } return data; } double *get_QD() const { return QD; } void swap_index(int i, int j) const { cache->swap_index(i,j); Kernel::swap_index(i,j); swap(QD[i],QD[j]); } ~ONE_CLASS_Q() { delete cache; delete[] QD; } private: Cache *cache; double *QD; }; class SVR_Q: public Kernel { public: SVR_Q(const svm_problem& prob, const svm_parameter& param) :Kernel(prob.l, prob.x, param) { l = prob.l; cache = new Cache(l,(long int)(param.cache_size*(1<<20))); QD = new double[2*l]; sign = new schar[2*l]; index = new int[2*l]; for(int k=0;k*kernel_function)(k,k); QD[k+l] = QD[k]; } buffer[0] = new Qfloat[2*l]; buffer[1] = new Qfloat[2*l]; next_buffer = 0; } void swap_index(int i, int j) const { swap(sign[i],sign[j]); swap(index[i],index[j]); swap(QD[i],QD[j]); } Qfloat *get_Q(int i, int len) const { Qfloat *data; int j, real_i = index[i]; if(cache->get_data(real_i,&data,l) < l) { for(j=0;j*kernel_function)(real_i,j); } // reorder and copy Qfloat *buf = buffer[next_buffer]; next_buffer = 1 - next_buffer; schar si = sign[i]; for(j=0;j // // construct and solve various formulations // static void solve_c_svc( const svm_problem *prob, const svm_parameter* param, double *alpha, Solver::SolutionInfo* si, double Cp, double Cn) { int l = prob->l; double *minus_ones = new double[l]; schar *y = new schar[l]; double *C = new double[l]; int i; for(i=0;iy[i] > 0) { y[i] = +1; C[i] = prob->W[i]*Cp; } else { y[i] = -1; C[i] = prob->W[i]*Cn; } //std::cout << C[i] << " "; } //std::cout << std::endl; Solver s; s.Solve(l, SVC_Q(*prob,*param,y), minus_ones, y, alpha, C, param->eps, si, param->shrinking); /* double sum_alpha=0; for(i=0;il)); */ for(i=0;il; double nu = param->nu; schar *y = new schar[l]; double *C = new double[l]; for(i=0;iy[i]>0) y[i] = +1; else y[i] = -1; C[i] = prob->W[i]; } double nu_l = 0; for(i=0;ieps, si, param->shrinking); double r = si->r; info("C = %f\n",1/r); for(i=0;iupper_bound[i] /= r; } si->rho /= r; si->obj /= (r*r); delete[] C; delete[] y; delete[] zeros; } static void solve_one_class( const svm_problem *prob, const svm_parameter *param, double *alpha, Solver::SolutionInfo* si) { int l = prob->l; double *zeros = new double[l]; schar *ones = new schar[l]; double *C = new double[l]; int i; double nu_l = 0; for(i=0;iW[i]; nu_l += C[i] * param->nu; } i = 0; while(nu_l > 0) { alpha[i] = min(C[i],nu_l); nu_l -= alpha[i]; ++i; } for(;ieps, si, param->shrinking); delete[] C; delete[] zeros; delete[] ones; } static void solve_epsilon_svr( const svm_problem *prob, const svm_parameter *param, double *alpha, Solver::SolutionInfo* si) { int l = prob->l; double *alpha2 = new double[2*l]; double *linear_term = new double[2*l]; double *C = new double[2*l]; schar *y = new schar[2*l]; int i; for(i=0;ip - prob->y[i]; y[i] = 1; C[i] = prob->W[i]*param->C; alpha2[i+l] = 0; linear_term[i+l] = param->p + prob->y[i]; y[i+l] = -1; C[i+l] = prob->W[i]*param->C; } Solver s; s.Solve(2*l, SVR_Q(*prob,*param), linear_term, y, alpha2, C, param->eps, si, param->shrinking); double sum_alpha = 0; for(i=0;iC*l)); delete[] alpha2; delete[] linear_term; delete[] C; delete[] y; } static void solve_nu_svr( const svm_problem *prob, const svm_parameter *param, double *alpha, Solver::SolutionInfo* si) { int l = prob->l; double *C = new double[2*l]; double *alpha2 = new double[2*l]; double *linear_term = new double[2*l]; schar *y = new schar[2*l]; int i; double sum = 0; for(i=0;iW[i]*param->C; sum += C[i] * param->nu; } sum /= 2; for(i=0;iy[i]; y[i] = 1; linear_term[i+l] = prob->y[i]; y[i+l] = -1; } Solver_NU s; s.Solve(2*l, SVR_Q(*prob,*param), linear_term, y, alpha2, C, param->eps, si, param->shrinking); info("epsilon = %f\n",-si->r); for(i=0;il); Solver::SolutionInfo si; switch(param->svm_type) { case C_SVC: si.upper_bound = Malloc(double,prob->l); solve_c_svc(prob,param,alpha,&si,Cp,Cn); break; case NU_SVC: si.upper_bound = Malloc(double,prob->l); solve_nu_svc(prob,param,alpha,&si); break; case ONE_CLASS: si.upper_bound = Malloc(double,prob->l); solve_one_class(prob,param,alpha,&si); break; case EPSILON_SVR: si.upper_bound = Malloc(double,2*prob->l); solve_epsilon_svr(prob,param,alpha,&si); break; case NU_SVR: si.upper_bound = Malloc(double,2*prob->l); solve_nu_svr(prob,param,alpha,&si); break; } info("obj = %f, rho = %f\n",si.obj,si.rho); // output SVs int nSV = 0; int nBSV = 0; for(int i=0;il;i++) { if(fabs(alpha[i]) > 0) { ++nSV; if(prob->y[i] > 0) { if(fabs(alpha[i]) >= si.upper_bound[i]) ++nBSV; } else { if(fabs(alpha[i]) >= si.upper_bound[i]) ++nBSV; } } } free(si.upper_bound); info("nSV = %d, nBSV = %d\n",nSV,nBSV); decision_function f; f.alpha = alpha; f.rho = si.rho; return f; } // Platt's binary SVM Probablistic Output: an improvement from Lin et al. static void sigmoid_train( int l, const double *dec_values, const double *labels, double& A, double& B) { double prior1=0, prior0 = 0; int i; for (i=0;i 0) prior1+=1; else prior0+=1; int max_iter=100; // Maximal number of iterations double min_step=1e-10; // Minimal step taken in line search double sigma=1e-12; // For numerically strict PD of Hessian double eps=1e-5; double hiTarget=(prior1+1.0)/(prior1+2.0); double loTarget=1/(prior0+2.0); double *t=Malloc(double,l); double fApB,p,q,h11,h22,h21,g1,g2,det,dA,dB,gd,stepsize; double newA,newB,newf,d1,d2; int iter; // Initial Point and Initial Fun Value A=0.0; B=log((prior0+1.0)/(prior1+1.0)); double fval = 0.0; for (i=0;i0) t[i]=hiTarget; else t[i]=loTarget; fApB = dec_values[i]*A+B; if (fApB>=0) fval += t[i]*fApB + log(1+exp(-fApB)); else fval += (t[i] - 1)*fApB +log(1+exp(fApB)); } for (iter=0;iter= 0) { p=exp(-fApB)/(1.0+exp(-fApB)); q=1.0/(1.0+exp(-fApB)); } else { p=1.0/(1.0+exp(fApB)); q=exp(fApB)/(1.0+exp(fApB)); } d2=p*q; h11+=dec_values[i]*dec_values[i]*d2; h22+=d2; h21+=dec_values[i]*d2; d1=t[i]-p; g1+=dec_values[i]*d1; g2+=d1; } // Stopping Criteria if (fabs(g1)= min_step) { newA = A + stepsize * dA; newB = B + stepsize * dB; // New function value newf = 0.0; for (i=0;i= 0) newf += t[i]*fApB + log(1+exp(-fApB)); else newf += (t[i] - 1)*fApB +log(1+exp(fApB)); } // Check sufficient decrease if (newf=max_iter) info("Reaching maximal iterations in two-class probability estimates\n"); free(t); } static double sigmoid_predict(double decision_value, double A, double B) { double fApB = decision_value*A+B; // 1-p used later; avoid catastrophic cancellation if (fApB >= 0) return exp(-fApB)/(1.0+exp(-fApB)); else return 1.0/(1+exp(fApB)) ; } // Method 2 from the multiclass_prob paper by Wu, Lin, and Weng static void multiclass_probability(int k, double **r, double *p) { int t,j; int iter = 0, max_iter=max(100,k); double **Q=Malloc(double *,k); double *Qp=Malloc(double,k); double pQp, eps=0.005/k; for (t=0;tmax_error) max_error=error; } if (max_error=max_iter) info("Exceeds max_iter in multiclass_prob\n"); for(t=0;tl); double *dec_values = Malloc(double,prob->l); // random shuffle for(i=0;il;i++) perm[i]=i; for(i=0;il;i++) { int j = i+rand()%(prob->l-i); swap(perm[i],perm[j]); } for(i=0;il/nr_fold; int end = (i+1)*prob->l/nr_fold; int j,k; struct svm_problem subprob; subprob.l = prob->l-(end-begin); subprob.x = Malloc(struct svm_node*,subprob.l); subprob.y = Malloc(double,subprob.l); subprob.W = Malloc(double,subprob.l); k=0; for(j=0;jx[perm[j]]; subprob.y[k] = prob->y[perm[j]]; subprob.W[k] = prob->W[perm[j]]; ++k; } for(j=end;jl;j++) { subprob.x[k] = prob->x[perm[j]]; subprob.y[k] = prob->y[perm[j]]; subprob.W[k] = prob->W[perm[j]]; ++k; } int p_count=0,n_count=0; for(j=0;j0) p_count++; else n_count++; if(p_count==0 && n_count==0) for(j=begin;j 0 && n_count == 0) for(j=begin;j 0) for(j=begin;jx[perm[j]],&(dec_values[perm[j]])); // ensure +1 -1 order; reason not using CV subroutine dec_values[perm[j]] *= submodel->label[0]; } svm_free_and_destroy_model(&submodel); svm_destroy_param(&subparam); } free(subprob.x); free(subprob.y); free(subprob.W); } sigmoid_train(prob->l,dec_values,prob->y,probA,probB); free(dec_values); free(perm); } // Return parameter of a Laplace distribution static double svm_svr_probability( const svm_problem *prob, const svm_parameter *param) { int i; int nr_fold = 5; double *ymv = Malloc(double,prob->l); double mae = 0; svm_parameter newparam = *param; newparam.probability = 0; svm_cross_validation(prob,&newparam,nr_fold,ymv); for(i=0;il;i++) { ymv[i]=prob->y[i]-ymv[i]; mae += fabs(ymv[i]); } mae /= prob->l; double std=sqrt(2*mae*mae); int count=0; mae=0; for(i=0;il;i++) if (fabs(ymv[i]) > 5*std) count=count+1; else mae+=fabs(ymv[i]); mae /= (prob->l-count); info("Prob. model for test data: target value = predicted value + z,\nz: Laplace distribution e^(-|z|/sigma)/(2sigma),sigma= %g\n",mae); free(ymv); return mae; } // label: label name, start: begin of each class, count: #data of classes, perm: indices to the original data // perm, length l, must be allocated before calling this subroutine static void svm_group_classes(const svm_problem *prob, int *nr_class_ret, int **label_ret, int **start_ret, int **count_ret, int *perm) { int l = prob->l; int max_nr_class = 16; int nr_class = 0; int *label = Malloc(int,max_nr_class); int *count = Malloc(int,max_nr_class); int *data_label = Malloc(int,l); int i; for(i=0;iy[i]; int j; for(j=0;j 0. // static void remove_zero_weight(svm_problem *newprob, const svm_problem *prob) { int i; int l = 0; for(i=0;il;i++) if(prob->W[i] > 0) l++; *newprob = *prob; newprob->l = l; newprob->x = Malloc(svm_node*,l); newprob->y = Malloc(double,l); newprob->W = Malloc(double,l); int j = 0; for(i=0;il;i++) if(prob->W[i] > 0) { newprob->x[j] = prob->x[i]; newprob->y[j] = prob->y[i]; newprob->W[j] = prob->W[i]; j++; } } // // Interface functions // svm_model *svm_train(const svm_problem *prob, const svm_parameter *param) { svm_problem newprob; remove_zero_weight(&newprob, prob); prob = &newprob; svm_model *model = Malloc(svm_model,1); model->param = *param; model->free_sv = 0; // XXX if(param->svm_type == ONE_CLASS || param->svm_type == EPSILON_SVR || param->svm_type == NU_SVR) { // regression or one-class-svm model->nr_class = 2; model->label = NULL; model->nSV = NULL; model->probA = NULL; model->probB = NULL; model->sv_coef = Malloc(double *,1); if(param->probability && (param->svm_type == EPSILON_SVR || param->svm_type == NU_SVR)) { model->probA = Malloc(double,1); model->probA[0] = svm_svr_probability(prob,param); } decision_function f = svm_train_one(prob,param,0,0); model->rho = Malloc(double,1); model->rho[0] = f.rho; int nSV = 0; int i; for(i=0;il;i++) if(fabs(f.alpha[i]) > 0) ++nSV; model->l = nSV; model->SV = Malloc(svm_node *,nSV); model->sv_coef[0] = Malloc(double,nSV); model->sv_indices = Malloc(int,nSV); int j = 0; for(i=0;il;i++) if(fabs(f.alpha[i]) > 0) { model->SV[j] = prob->x[i]; model->sv_coef[0][j] = f.alpha[i]; model->sv_indices[j] = i+1; ++j; } free(f.alpha); } else { // classification int l = prob->l; int nr_class; int *label = NULL; int *start = NULL; int *count = NULL; int *perm = Malloc(int,l); // group training data of the same class svm_group_classes(prob,&nr_class,&label,&start,&count,perm); if(nr_class == 1) info("WARNING: training data in only one class. See README for details.\n"); svm_node **x = Malloc(svm_node *,l); double *W; W = Malloc(double,l); int i; for(i=0;ix[perm[i]]; W[i] = prob->W[perm[i]]; } // calculate weighted C double *weighted_C = Malloc(double, nr_class); for(i=0;iC; for(i=0;inr_weight;i++) { int j; for(j=0;jweight_label[i] == label[j]) break; if(j == nr_class) fprintf(stderr,"WARNING: class label %d specified in weight is not found\n", param->weight_label[i]); else weighted_C[j] *= param->weight[i]; } // train k*(k-1)/2 models bool *nonzero = Malloc(bool,l); for(i=0;iprobability) { probA=Malloc(double,nr_class*(nr_class-1)/2); probB=Malloc(double,nr_class*(nr_class-1)/2); } int p = 0; for(i=0;iprobability) svm_binary_svc_probability(&sub_prob,param,weighted_C[i],weighted_C[j],probA[p],probB[p]); f[p] = svm_train_one(&sub_prob,param,weighted_C[i],weighted_C[j]); for(k=0;k 0) nonzero[si+k] = true; for(k=0;k 0) nonzero[sj+k] = true; free(sub_prob.x); free(sub_prob.y); free(sub_prob.W); ++p; } // build output model->nr_class = nr_class; model->label = Malloc(int,nr_class); for(i=0;ilabel[i] = label[i]; model->rho = Malloc(double,nr_class*(nr_class-1)/2); for(i=0;irho[i] = f[i].rho; if(param->probability) { model->probA = Malloc(double,nr_class*(nr_class-1)/2); model->probB = Malloc(double,nr_class*(nr_class-1)/2); for(i=0;iprobA[i] = probA[i]; model->probB[i] = probB[i]; } } else { model->probA=NULL; model->probB=NULL; } int total_sv = 0; int *nz_count = Malloc(int,nr_class); model->nSV = Malloc(int,nr_class); for(i=0;inSV[i] = nSV; nz_count[i] = nSV; } info("Total nSV = %d\n",total_sv); model->l = total_sv; model->SV = Malloc(svm_node *,total_sv); model->sv_indices = Malloc(int,total_sv); p = 0; for(i=0;iSV[p] = x[i]; model->sv_indices[p++] = perm[i] + 1; } int *nz_start = Malloc(int,nr_class); nz_start[0] = 0; for(i=1;isv_coef = Malloc(double *,nr_class-1); for(i=0;isv_coef[i] = Malloc(double,total_sv); p = 0; for(i=0;isv_coef[j-1][q++] = f[p].alpha[k]; q = nz_start[j]; for(k=0;ksv_coef[i][q++] = f[p].alpha[ci+k]; ++p; } free(label); free(probA); free(probB); free(count); free(perm); free(start); free(W); free(x); free(weighted_C); free(nonzero); for(i=0;il; int *perm = Malloc(int,l); int nr_class; // stratified cv may not give leave-one-out rate // Each class to l folds -> some folds may have zero elements if((param->svm_type == C_SVC || param->svm_type == NU_SVC) && nr_fold < l) { int *start = NULL; int *label = NULL; int *count = NULL; svm_group_classes(prob,&nr_class,&label,&start,&count,perm); // random shuffle and then data grouped by fold using the array perm int *fold_count = Malloc(int,nr_fold); int c; int *index = Malloc(int,l); for(i=0;ix[perm[j]]; subprob.y[k] = prob->y[perm[j]]; subprob.W[k] = prob->W[perm[j]]; ++k; } for(j=end;jx[perm[j]]; subprob.y[k] = prob->y[perm[j]]; subprob.W[k] = prob->W[perm[j]]; ++k; } struct svm_model *submodel = svm_train(&subprob,param); if(param->probability && (param->svm_type == C_SVC || param->svm_type == NU_SVC)) { double *prob_estimates=Malloc(double,svm_get_nr_class(submodel)); for(j=begin;jx[perm[j]],prob_estimates); free(prob_estimates); } else for(j=begin;jx[perm[j]]); svm_free_and_destroy_model(&submodel); free(subprob.x); free(subprob.y); free(subprob.W); } free(fold_start); free(perm); } int svm_get_svm_type(const svm_model *model) { return model->param.svm_type; } int svm_get_nr_class(const svm_model *model) { return model->nr_class; } void svm_get_labels(const svm_model *model, int* label) { if (model->label != NULL) for(int i=0;inr_class;i++) label[i] = model->label[i]; } void svm_get_sv_indices(const svm_model *model, int* indices) { if (model->sv_indices != NULL) for(int i=0;il;i++) indices[i] = model->sv_indices[i]; } int svm_get_nr_sv(const svm_model *model) { return model->l; } double svm_get_svr_probability(const svm_model *model) { if ((model->param.svm_type == EPSILON_SVR || model->param.svm_type == NU_SVR) && model->probA!=NULL) return model->probA[0]; else { fprintf(stderr,"Model doesn't contain information for SVR probability inference\n"); return 0; } } double svm_hyper_w_normsqr_twoclass(const struct svm_model* model) { assert(model->param.svm_type == C_SVC || model->param.svm_type == NU_SVC); int i, j; // int nr_class = model->nr_class; // assert(nr_class == 2); int l = model->l; double sum = 0; double *coef = model->sv_coef[0]; for(i=0;iSV[i],model->SV[j],model->param); sum += kv * coef[i] * coef[j]; } return sum; } double svm_predict_values_twoclass(const struct svm_model* model, const struct svm_node* x) { assert(model->param.svm_type == C_SVC || model->param.svm_type == NU_SVC); int i; // int nr_class = model->nr_class; // assert(nr_class == 2); int l = model->l; double *kvalue = Malloc(double,l); for(i=0;iSV[i],model->param); double sum = 0; double *coef = model->sv_coef[0]; for(i=0;irho[0]; free(kvalue); return sum * model->label[0]; } double svm_predict_values(const svm_model *model, const svm_node *x, double* dec_values) { int i; if(model->param.svm_type == ONE_CLASS || model->param.svm_type == EPSILON_SVR || model->param.svm_type == NU_SVR) { double *sv_coef = model->sv_coef[0]; double sum = 0; for(i=0;il;i++) sum += sv_coef[i] * Kernel::k_function(x,model->SV[i],model->param); sum -= model->rho[0]; *dec_values = sum; if(model->param.svm_type == ONE_CLASS) return (sum>0)?1:-1; else return sum; } else { int nr_class = model->nr_class; int l = model->l; double *kvalue = Malloc(double,l); for(i=0;iSV[i],model->param); int *start = Malloc(int,nr_class); start[0] = 0; for(i=1;inSV[i-1]; int *vote = Malloc(int,nr_class); for(i=0;inSV[i]; int cj = model->nSV[j]; int k; double *coef1 = model->sv_coef[j-1]; double *coef2 = model->sv_coef[i]; for(k=0;krho[p]; dec_values[p] = sum; if(dec_values[p] > 0) ++vote[i]; else ++vote[j]; p++; } int vote_max_idx = 0; for(i=1;i vote[vote_max_idx]) vote_max_idx = i; free(kvalue); free(start); free(vote); return model->label[vote_max_idx]; } } double svm_predict(const svm_model *model, const svm_node *x) { int nr_class = model->nr_class; double *dec_values; if(model->param.svm_type == ONE_CLASS || model->param.svm_type == EPSILON_SVR || model->param.svm_type == NU_SVR) dec_values = Malloc(double, 1); else dec_values = Malloc(double, nr_class*(nr_class-1)/2); double pred_result = svm_predict_values(model, x, dec_values); free(dec_values); return pred_result; } double svm_predict_probability( const svm_model *model, const svm_node *x, double *prob_estimates) { if ((model->param.svm_type == C_SVC || model->param.svm_type == NU_SVC) && model->probA!=NULL && model->probB!=NULL) { int i; int nr_class = model->nr_class; double *dec_values = Malloc(double, nr_class*(nr_class-1)/2); svm_predict_values(model, x, dec_values); double min_prob=1e-7; double **pairwise_prob=Malloc(double *,nr_class); for(i=0;iprobA[k],model->probB[k]),min_prob),1-min_prob); pairwise_prob[j][i]=1-pairwise_prob[i][j]; k++; } multiclass_probability(nr_class,pairwise_prob,prob_estimates); int prob_max_idx = 0; for(i=1;i prob_estimates[prob_max_idx]) prob_max_idx = i; for(i=0;ilabel[prob_max_idx]; } else return svm_predict(model, x); } static const char *svm_type_table[] = { "c_svc","nu_svc","one_class","epsilon_svr","nu_svr",NULL }; static const char *kernel_type_table[]= { "linear","polynomial","rbf","sigmoid","precomputed",NULL }; int svm_save_model(const char *model_file_name, const svm_model *model) { FILE *fp = fopen(model_file_name,"w"); if(fp==NULL) return -1; char *old_locale = strdup(setlocale(LC_ALL, NULL)); setlocale(LC_ALL, "C"); const svm_parameter& param = model->param; fprintf(fp,"svm_type %s\n", svm_type_table[param.svm_type]); fprintf(fp,"kernel_type %s\n", kernel_type_table[param.kernel_type]); if(param.kernel_type == POLY) fprintf(fp,"degree %d\n", param.degree); if(param.kernel_type == POLY || param.kernel_type == RBF || param.kernel_type == SIGMOID) fprintf(fp,"gamma %g\n", param.gamma); if(param.kernel_type == POLY || param.kernel_type == SIGMOID) fprintf(fp,"coef0 %g\n", param.coef0); int nr_class = model->nr_class; int l = model->l; fprintf(fp, "nr_class %d\n", nr_class); fprintf(fp, "total_sv %d\n",l); { fprintf(fp, "rho"); for(int i=0;irho[i]); fprintf(fp, "\n"); } if(model->label) { fprintf(fp, "label"); for(int i=0;ilabel[i]); fprintf(fp, "\n"); } if(model->probA) // regression has probA only { fprintf(fp, "probA"); for(int i=0;iprobA[i]); fprintf(fp, "\n"); } if(model->probB) { fprintf(fp, "probB"); for(int i=0;iprobB[i]); fprintf(fp, "\n"); } if(model->nSV) { fprintf(fp, "nr_sv"); for(int i=0;inSV[i]); fprintf(fp, "\n"); } fprintf(fp, "SV\n"); const double * const *sv_coef = model->sv_coef; const svm_node * const *SV = model->SV; for(int i=0;ivalue)); else while(p->index != -1) { fprintf(fp,"%d:%.8g ",p->index,p->value); p++; } fprintf(fp, "\n"); } setlocale(LC_ALL, old_locale); free(old_locale); if (ferror(fp) != 0 || fclose(fp) != 0) return -1; else return 0; } static char *line = NULL; static int max_line_len; static char* readline(FILE *input) { int len; if(fgets(line,max_line_len,input) == NULL) return NULL; while(strrchr(line,'\n') == NULL) { max_line_len *= 2; line = (char *) realloc(line,max_line_len); len = (int) strlen(line); if(fgets(line+len,max_line_len-len,input) == NULL) break; } return line; } svm_model *svm_load_model(const char *model_file_name) { FILE *fp = fopen(model_file_name,"rb"); if(fp==NULL) return NULL; char *old_locale = strdup(setlocale(LC_ALL, NULL)); setlocale(LC_ALL, "C"); // read parameters svm_model *model = Malloc(svm_model,1); svm_parameter& param = model->param; model->rho = NULL; model->probA = NULL; model->probB = NULL; model->label = NULL; model->nSV = NULL; char cmd[81]; while(1) { fscanf(fp,"%80s",cmd); if(strcmp(cmd,"svm_type")==0) { fscanf(fp,"%80s",cmd); int i; for(i=0;svm_type_table[i];i++) { if(strcmp(svm_type_table[i],cmd)==0) { param.svm_type=i; break; } } if(svm_type_table[i] == NULL) { fprintf(stderr,"unknown svm type.\n"); setlocale(LC_ALL, old_locale); free(old_locale); free(model->rho); free(model->label); free(model->nSV); free(model); return NULL; } } else if(strcmp(cmd,"kernel_type")==0) { fscanf(fp,"%80s",cmd); int i; for(i=0;kernel_type_table[i];i++) { if(strcmp(kernel_type_table[i],cmd)==0) { param.kernel_type=i; break; } } if(kernel_type_table[i] == NULL) { fprintf(stderr,"unknown kernel function.\n"); setlocale(LC_ALL, old_locale); free(old_locale); free(model->rho); free(model->label); free(model->nSV); free(model); return NULL; } } else if(strcmp(cmd,"degree")==0) fscanf(fp,"%d",¶m.degree); else if(strcmp(cmd,"gamma")==0) fscanf(fp,"%lf",¶m.gamma); else if(strcmp(cmd,"coef0")==0) fscanf(fp,"%lf",¶m.coef0); else if(strcmp(cmd,"nr_class")==0) fscanf(fp,"%d",&model->nr_class); else if(strcmp(cmd,"total_sv")==0) fscanf(fp,"%d",&model->l); else if(strcmp(cmd,"rho")==0) { int n = model->nr_class * (model->nr_class-1)/2; model->rho = Malloc(double,n); for(int i=0;irho[i]); } else if(strcmp(cmd,"label")==0) { int n = model->nr_class; model->label = Malloc(int,n); for(int i=0;ilabel[i]); } else if(strcmp(cmd,"probA")==0) { int n = model->nr_class * (model->nr_class-1)/2; model->probA = Malloc(double,n); for(int i=0;iprobA[i]); } else if(strcmp(cmd,"probB")==0) { int n = model->nr_class * (model->nr_class-1)/2; model->probB = Malloc(double,n); for(int i=0;iprobB[i]); } else if(strcmp(cmd,"nr_sv")==0) { int n = model->nr_class; model->nSV = Malloc(int,n); for(int i=0;inSV[i]); } else if(strcmp(cmd,"SV")==0) { while(1) { int c = getc(fp); if(c==EOF || c=='\n') break; } break; } else { fprintf(stderr,"unknown text in model file: [%s]\n",cmd); setlocale(LC_ALL, old_locale); free(old_locale); free(model->rho); free(model->label); free(model->nSV); free(model); return NULL; } } // read sv_coef and SV int elements = 0; long pos = ftell(fp); max_line_len = 1024; line = Malloc(char,max_line_len); char *p,*endptr,*idx,*val; while(readline(fp)!=NULL) { p = strtok(line,":"); while(1) { p = strtok(NULL,":"); if(p == NULL) break; ++elements; } } elements += model->l; fseek(fp,pos,SEEK_SET); int m = model->nr_class - 1; int l = model->l; model->sv_coef = Malloc(double *,m); int i; for(i=0;isv_coef[i] = Malloc(double,l); model->SV = Malloc(svm_node*,l); svm_node *x_space = NULL; if(l>0) x_space = Malloc(svm_node,elements); int j=0; for(i=0;iSV[i] = &x_space[j]; p = strtok(line, " \t"); model->sv_coef[0][i] = strtod(p,&endptr); for(int k=1;ksv_coef[k][i] = strtod(p,&endptr); } while(1) { idx = strtok(NULL, ":"); val = strtok(NULL, " \t"); if(val == NULL) break; x_space[j].index = (int) strtol(idx,&endptr,10); x_space[j].value = strtod(val,&endptr); ++j; } x_space[j++].index = -1; } free(line); setlocale(LC_ALL, old_locale); free(old_locale); if (ferror(fp) != 0 || fclose(fp) != 0) return NULL; model->free_sv = 1; // XXX return model; } void svm_free_model_content(svm_model* model_ptr) { if(model_ptr->free_sv && model_ptr->l > 0 && model_ptr->SV != NULL) free((void *)(model_ptr->SV[0])); if(model_ptr->sv_coef) { for(int i=0;inr_class-1;i++) free(model_ptr->sv_coef[i]); } free(model_ptr->SV); model_ptr->SV = NULL; free(model_ptr->sv_coef); model_ptr->sv_coef = NULL; free(model_ptr->rho); model_ptr->rho = NULL; free(model_ptr->label); model_ptr->label= NULL; free(model_ptr->probA); model_ptr->probA = NULL; free(model_ptr->probB); model_ptr->probB= NULL; free(model_ptr->nSV); model_ptr->nSV = NULL; } void svm_free_and_destroy_model(svm_model** model_ptr_ptr) { if(model_ptr_ptr != NULL && *model_ptr_ptr != NULL) { svm_free_model_content(*model_ptr_ptr); free(*model_ptr_ptr); *model_ptr_ptr = NULL; } } void svm_destroy_param(svm_parameter* param) { free(param->weight_label); free(param->weight); } const char *svm_check_parameter(const svm_problem *prob, const svm_parameter *param) { // svm_type int svm_type = param->svm_type; if(svm_type != C_SVC && svm_type != NU_SVC && svm_type != ONE_CLASS && svm_type != EPSILON_SVR && svm_type != NU_SVR) return "unknown svm type"; // kernel_type, degree int kernel_type = param->kernel_type; if(kernel_type != LINEAR && kernel_type != POLY && kernel_type != RBF && kernel_type != SIGMOID && kernel_type != PRECOMPUTED) return "unknown kernel type"; if(param->gamma < 0) return "gamma < 0"; if(param->degree < 0) return "degree of polynomial kernel < 0"; // cache_size,eps,C,nu,p,shrinking if(param->cache_size <= 0) return "cache_size <= 0"; if(param->eps <= 0) return "eps <= 0"; if(svm_type == C_SVC || svm_type == EPSILON_SVR || svm_type == NU_SVR) if(param->C <= 0) return "C <= 0"; if(svm_type == NU_SVC || svm_type == ONE_CLASS || svm_type == NU_SVR) if(param->nu <= 0 || param->nu > 1) return "nu <= 0 or nu > 1"; if(svm_type == EPSILON_SVR) if(param->p < 0) return "p < 0"; if(param->shrinking != 0 && param->shrinking != 1) return "shrinking != 0 and shrinking != 1"; if(param->probability != 0 && param->probability != 1) return "probability != 0 and probability != 1"; if(param->probability == 1 && svm_type == ONE_CLASS) return "one-class SVM probability output not supported yet"; // check whether nu-svc is feasible if(svm_type == NU_SVC) { int l = prob->l; int max_nr_class = 16; int nr_class = 0; int *label = Malloc(int,max_nr_class); double *count = Malloc(double,max_nr_class); int i; for(i=0;iy[i]; int j; for(j=0;jW[i]; break; } if(j == nr_class) { if(nr_class == max_nr_class) { max_nr_class *= 2; label = (int *)realloc(label,max_nr_class*sizeof(int)); count = (double *)realloc(count,max_nr_class*sizeof(double)); } label[nr_class] = this_label; count[nr_class] = prob->W[i]; ++nr_class; } } for(i=0;inu*(n1+n2)/2 > min(n1,n2)) { free(label); free(count); return "specified nu is infeasible"; } } } free(label); free(count); } return NULL; } int svm_check_probability_model(const svm_model *model) { return ((model->param.svm_type == C_SVC || model->param.svm_type == NU_SVC) && model->probA!=NULL && model->probB!=NULL) || ((model->param.svm_type == EPSILON_SVR || model->param.svm_type == NU_SVR) && model->probA!=NULL); } void svm_set_print_string_function(void (*print_func)(const char *)) { if(print_func == NULL) svm_print_string = &print_string_stdout; else svm_print_string = print_func; } fcl-0.5.0/test/libsvm/svm.h000066400000000000000000000072671274356571000155370ustar00rootroot00000000000000#ifndef _LIBSVM_H #define _LIBSVM_H #include #define LIBSVM_VERSION 314 #ifdef __cplusplus extern "C" { #endif extern int libsvm_version; struct svm_node { int index; double value; }; struct svm_problem { int l; double *y; struct svm_node **x; double *W; /* instance weight */ }; enum { C_SVC, NU_SVC, ONE_CLASS, EPSILON_SVR, NU_SVR }; /* svm_type */ enum { LINEAR, POLY, RBF, SIGMOID, PRECOMPUTED }; /* kernel_type */ struct svm_parameter { int svm_type; int kernel_type; int degree; /* for poly */ double gamma; /* for poly/rbf/sigmoid */ double coef0; /* for poly/sigmoid */ /* these are for training only */ double cache_size; /* in MB */ double eps; /* stopping criteria */ double C; /* for C_SVC, EPSILON_SVR and NU_SVR */ int nr_weight; /* for C_SVC */ int *weight_label; /* for C_SVC */ double* weight; /* for C_SVC */ double nu; /* for NU_SVC, ONE_CLASS, and NU_SVR */ double p; /* for EPSILON_SVR */ int shrinking; /* use the shrinking heuristics */ int probability; /* do probability estimates */ }; // // svm_model // struct svm_model { struct svm_parameter param; /* parameter */ int nr_class; /* number of classes, = 2 in regression/one class svm */ int l; /* total #SV */ struct svm_node **SV; /* SVs (SV[l]) */ double **sv_coef; /* coefficients for SVs in decision functions (sv_coef[k-1][l]) */ double *rho; /* constants in decision functions (rho[k*(k-1)/2]) */ double *probA; /* pairwise probability information */ double *probB; int *sv_indices; /* sv_indices[0,...,nSV-1] are values in [1,...,num_traning_data] to indicate SVs in the training set */ /* for classification only */ int *label; /* label of each class (label[k]) */ int *nSV; /* number of SVs for each class (nSV[k]) */ /* nSV[0] + nSV[1] + ... + nSV[k-1] = l */ /* XXX */ int free_sv; /* 1 if svm_model is created by svm_load_model*/ /* 0 if svm_model is created by svm_train */ }; struct svm_model *svm_train(const struct svm_problem *prob, const struct svm_parameter *param); void svm_cross_validation(const struct svm_problem *prob, const struct svm_parameter *param, int nr_fold, double *target); int svm_save_model(const char *model_file_name, const struct svm_model *model); struct svm_model *svm_load_model(const char *model_file_name); int svm_get_svm_type(const struct svm_model *model); int svm_get_nr_class(const struct svm_model *model); void svm_get_labels(const struct svm_model *model, int *label); void svm_get_sv_indices(const struct svm_model *model, int *sv_indices); int svm_get_nr_sv(const struct svm_model *model); double svm_get_svr_probability(const struct svm_model *model); double svm_predict_values(const struct svm_model *model, const struct svm_node *x, double* dec_values); double svm_predict(const struct svm_model *model, const struct svm_node *x); double svm_predict_probability(const struct svm_model *model, const struct svm_node *x, double* prob_estimates); double svm_predict_values_twoclass(const struct svm_model* model, const struct svm_node* x); double svm_hyper_w_normsqr_twoclass(const struct svm_model* model); double k_function(const svm_node* x, const svm_node* y, const svm_parameter& param); void svm_free_model_content(struct svm_model *model_ptr); void svm_free_and_destroy_model(struct svm_model **model_ptr_ptr); void svm_destroy_param(struct svm_parameter *param); const char *svm_check_parameter(const struct svm_problem *prob, const struct svm_parameter *param); int svm_check_probability_model(const struct svm_model *model); void svm_set_print_string_function(void (*print_func)(const char *)); #ifdef __cplusplus } #endif #endif /* _LIBSVM_H */ fcl-0.5.0/test/libsvm_classifier.h000066400000000000000000000120601274356571000171210ustar00rootroot00000000000000#ifndef FCL_TEST_LIBSVM_CLASSIFIER_H #define FCL_TEST_LIBSVM_CLASSIFIER_H #include "fcl/learning/classifier.h" #include namespace fcl { template class LibSVMClassifier : public SVMClassifier { public: LibSVMClassifier() { param.svm_type = C_SVC; param.kernel_type = RBF; param.degree = 3; param.gamma = 0; // 1/num_features param.coef0 = 0; param.nu = 0.5; param.cache_size = 100; // can change param.C = 1; param.eps = 1e-3; param.p = 0.1; param.shrinking = 1; // use shrinking param.probability = 0; param.nr_weight = 0; param.weight_label = NULL; param.weight = NULL; param.nr_weight = 2; param.weight_label = (int *)realloc(param.weight_label, sizeof(int) * param.nr_weight); param.weight = (double *)realloc(param.weight, sizeof(double) * param.nr_weight); param.weight_label[0] = -1; param.weight_label[1] = 1; param.weight[0] = 1; param.weight[1] = 1; model = NULL; x_space = NULL; problem.x = NULL; problem.y = NULL; problem.W = NULL; } void setCSVM() { param.svm_type = C_SVC; } void setNuSVM() { param.svm_type = NU_SVC; } void setC(FCL_REAL C) { param.C = C; } void setNu(FCL_REAL nu) { param.nu = nu; } void setLinearClassifier() { param.kernel_type = LINEAR; } void setNonLinearClassifier() { param.kernel_type = RBF; } void setProbability(bool use_probability) { param.probability = use_probability; } virtual void setScaler(const Scaler& scaler_) { scaler = scaler_; } void setNegativeWeight(FCL_REAL c) { param.weight[0] = c; } void setPositiveWeight(FCL_REAL c) { param.weight[1] = c; } void setEPS(FCL_REAL e) { param.eps = e; } void setGamma(FCL_REAL gamma) { param.gamma = gamma; } ~LibSVMClassifier() { svm_destroy_param(¶m); svm_free_and_destroy_model(&model); delete [] x_space; delete [] problem.x; delete [] problem.y; delete [] problem.W; } virtual void learn(const std::vector >& data) { if(data.size() == 0) return; if(model) svm_free_and_destroy_model(&model); if(param.gamma == 0) param.gamma = 1.0 / N; problem.l = data.size(); if(problem.y) delete [] problem.y; problem.y = new double [problem.l]; if(problem.x) delete [] problem.x; problem.x = new svm_node* [problem.l]; if(problem.W) delete [] problem.W; problem.W = new double [problem.l]; if(x_space) delete [] x_space; x_space = new svm_node [(N + 1) * problem.l]; for(std::size_t i = 0; i < data.size(); ++i) { svm_node* cur_x_space = x_space + (N + 1) * i; Vecnf q_scaled = scaler.scale(data[i].q); for(std::size_t j = 0; j < N; ++j) { cur_x_space[j].index = j + 1; cur_x_space[j].value = q_scaled[j]; } cur_x_space[N].index = -1; problem.x[i] = cur_x_space; problem.y[i] = (data[i].label ? 1 : -1); problem.W[i] = data[i].w; } model = svm_train(&problem, ¶m); hyperw_normsqr = svm_hyper_w_normsqr_twoclass(model); } virtual std::vector predict(const std::vector >& qs) const { std::vector predict_results; int nr_class = svm_get_nr_class(model); double* prob_estimates = NULL; svm_node* x = (svm_node*)malloc((N + 1) * sizeof(svm_node)); if(param.probability) prob_estimates = (double*)malloc(nr_class * sizeof(double)); Vecnf v; for(std::size_t i = 0; i < qs.size(); ++i) { v = scaler.scale(qs[i]); for(std::size_t j = 0; j < N; ++j) { x[j].index = j + 1; x[j].value = v[j]; } x[N].index = -1; double predict_label; if(param.probability) { predict_label = svm_predict_probability(model, x, prob_estimates); predict_label = (predict_label > 0) ? 1 : 0; predict_results.push_back(PredictResult(predict_label, *prob_estimates)); } else { predict_label = svm_predict(model, x); predict_label = (predict_label > 0) ? 1 : 0; predict_results.push_back(PredictResult(predict_label)); } } if(param.probability) free(prob_estimates); free(x); return predict_results; } virtual PredictResult predict(const Vecnf& q) const { return (predict(std::vector >(1, q)))[0]; } void save(const std::string& filename) const { if(model) svm_save_model(filename.c_str(), model); } virtual std::vector > getSupportVectors() const { std::vector > results; Item item; for(std::size_t i = 0; i < (std::size_t)model->l; ++i) { for(std::size_t j = 0; j < N; ++j) item.q[j] = model->SV[i][j].value; item.q = scaler.unscale(item.q); int id = model->sv_indices[i] - 1; item.label = (problem.y[id] > 0); results.push_back(item); } return results; } svm_parameter param; svm_problem problem; svm_node* x_space; svm_model* model; double hyperw_normsqr; Scaler scaler; }; } #endif fcl-0.5.0/test/test_fcl_broadphase.cpp000066400000000000000000001301121274356571000177460ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #define BOOST_TEST_MODULE "FCL_BROADPHASE" #include #include "fcl/config.h" #include "fcl/broadphase/broadphase.h" #include "fcl/shape/geometric_shape_to_BVH_model.h" #include "fcl/math/transform.h" #include "test_fcl_utility.h" #if USE_GOOGLEHASH #include #include #include #endif #include #include using namespace fcl; struct TStruct { std::vector records; double overall_time; TStruct() { overall_time = 0; } void push_back(double t) { records.push_back(t); overall_time += t; } }; /// @brief Generate environment with 3 * n objects: n boxes, n spheres and n cylinders. void generateEnvironments(std::vector& env, double env_scale, std::size_t n); /// @brief Generate environment with 3 * n objects for self distance, so we try to make sure none of them collide with each other. void generateSelfDistanceEnvironments(std::vector& env, double env_scale, std::size_t n); /// @brief Generate environment with 3 * n objects, but all in meshes. void generateEnvironmentsMesh(std::vector& env, double env_scale, std::size_t n); /// @brief Generate environment with 3 * n objects for self distance, but all in meshes. void generateSelfDistanceEnvironmentsMesh(std::vector& env, double env_scale, std::size_t n); /// @brief test for broad phase collision and self collision void broad_phase_collision_test(double env_scale, std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts = 1, bool exhaustive = false, bool use_mesh = false); /// @brief test for broad phase distance void broad_phase_distance_test(double env_scale, std::size_t env_size, std::size_t query_size, bool use_mesh = false); /// @brief test for broad phase self distance void broad_phase_self_distance_test(double env_scale, std::size_t env_size, bool use_mesh = false); /// @brief test for broad phase update void broad_phase_update_collision_test(double env_scale, std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts = 1, bool exhaustive = false, bool use_mesh = false); FCL_REAL DELTA = 0.01; #if USE_GOOGLEHASH template struct GoogleSparseHashTable : public google::sparse_hash_map, std::equal_to > {}; template struct GoogleDenseHashTable : public google::dense_hash_map, std::equal_to > { GoogleDenseHashTable() : google::dense_hash_map, std::equal_to >() { this->set_empty_key(NULL); } }; #endif /// check the update, only return collision or not BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_update_collision_binary) { broad_phase_update_collision_test(2000, 100, 1000, 1, false); broad_phase_update_collision_test(2000, 1000, 1000, 1, false); } /// check the update, return 10 contacts BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_update_collision) { broad_phase_update_collision_test(2000, 100, 1000, 10, false); broad_phase_update_collision_test(2000, 1000, 1000, 10, false); } /// check the update, exhaustive BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_update_collision_exhaustive) { broad_phase_update_collision_test(2000, 100, 1000, 1, true); broad_phase_update_collision_test(2000, 1000, 1000, 1, true); } /// check broad phase distance BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_distance) { broad_phase_distance_test(200, 100, 100); broad_phase_distance_test(200, 1000, 100); broad_phase_distance_test(2000, 100, 100); broad_phase_distance_test(2000, 1000, 100); } /// check broad phase self distance BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_self_distance) { broad_phase_self_distance_test(200, 512); broad_phase_self_distance_test(200, 1000); broad_phase_self_distance_test(200, 5000); } /// check broad phase collision for empty collision object set and queries BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision_empty) { broad_phase_collision_test(2000, 0, 0, 10, false, false); broad_phase_collision_test(2000, 0, 1000, 10, false, false); broad_phase_collision_test(2000, 100, 0, 10, false, false); broad_phase_collision_test(2000, 0, 0, 10, false, true); broad_phase_collision_test(2000, 0, 1000, 10, false, true); broad_phase_collision_test(2000, 100, 0, 10, false, true); broad_phase_collision_test(2000, 0, 0, 10, true, false); broad_phase_collision_test(2000, 0, 1000, 10, true, false); broad_phase_collision_test(2000, 100, 0, 10, true, false); broad_phase_collision_test(2000, 0, 0, 10, true, true); broad_phase_collision_test(2000, 0, 1000, 10, true, true); broad_phase_collision_test(2000, 100, 0, 10, true, true); } /// check broad phase collision and self collision, only return collision or not BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision_binary) { #ifdef FCL_BUILD_TYPE_DEBUG broad_phase_collision_test(2000, 10, 100, 1, false); broad_phase_collision_test(2000, 100, 100, 1, false); broad_phase_collision_test(2000, 10, 100, 1, true); broad_phase_collision_test(2000, 100, 100, 1, true); #else broad_phase_collision_test(2000, 100, 1000, 1, false); broad_phase_collision_test(2000, 1000, 1000, 1, false); broad_phase_collision_test(2000, 100, 1000, 1, true); broad_phase_collision_test(2000, 1000, 1000, 1, true); #endif } /// check broad phase collision and self collision, return 10 contacts BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision) { broad_phase_collision_test(2000, 100, 1000, 10, false); broad_phase_collision_test(2000, 1000, 1000, 10, false); } /// check broad phase update, in mesh, only return collision or not BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_update_collision_mesh_binary) { broad_phase_update_collision_test(2000, 100, 1000, false, true); broad_phase_update_collision_test(2000, 1000, 1000, false, true); } /// check broad phase update, in mesh, return 10 contacts BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_update_collision_mesh) { #ifdef FCL_BUILD_TYPE_DEBUG broad_phase_update_collision_test(200, 10, 100, 10, false, true); broad_phase_update_collision_test(200, 100, 100, 10, false, true); #else broad_phase_update_collision_test(2000, 100, 1000, 10, false, true); broad_phase_update_collision_test(2000, 1000, 1000, 10, false, true); #endif } /// check broad phase update, in mesh, exhaustive BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_update_collision_mesh_exhaustive) { #ifdef FCL_BUILD_TYPE_DEBUG broad_phase_update_collision_test(2000, 10, 100, 1, true, true); broad_phase_update_collision_test(2000, 100, 100, 1, true, true); #else broad_phase_update_collision_test(2000, 100, 1000, 1, true, true); broad_phase_update_collision_test(2000, 1000, 1000, 1, true, true); #endif } /// check broad phase distance BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_distance_mesh) { #ifdef FCL_BUILD_TYPE_DEBUG broad_phase_distance_test(200, 10, 10, true); broad_phase_distance_test(200, 100, 10, true); broad_phase_distance_test(2000, 10, 10, true); broad_phase_distance_test(2000, 100, 10, true); #else broad_phase_distance_test(200, 100, 100, true); broad_phase_distance_test(200, 1000, 100, true); broad_phase_distance_test(2000, 100, 100, true); broad_phase_distance_test(2000, 1000, 100, true); #endif } /// check broad phase self distance BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_self_distance_mesh) { broad_phase_self_distance_test(200, 512, true); broad_phase_self_distance_test(200, 1000, true); broad_phase_self_distance_test(200, 5000, true); } /// check broad phase collision and self collision, return only collision or not, in mesh BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh_binary) { #ifdef FCL_BUILD_TYPE_DEBUG broad_phase_collision_test(2000, 10, 100, 1, false, true); broad_phase_collision_test(2000, 100, 100, 1, false, true); #else broad_phase_collision_test(2000, 100, 1000, 1, false, true); broad_phase_collision_test(2000, 1000, 1000, 1, false, true); #endif } /// check broad phase collision and self collision, return 10 contacts, in mesh BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh) { #ifdef FCL_BUILD_TYPE_DEBUG broad_phase_collision_test(2000, 10, 100, 10, false, true); broad_phase_collision_test(2000, 100, 100, 10, false, true); #else broad_phase_collision_test(2000, 100, 1000, 10, false, true); broad_phase_collision_test(2000, 1000, 1000, 10, false, true); #endif } /// check broad phase collision and self collision, exhaustive, in mesh BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh_exhaustive) { #ifdef FCL_BUILD_TYPE_DEBUG broad_phase_collision_test(2000, 10, 100, 1, true, true); broad_phase_collision_test(2000, 100, 100, 1, true, true); #else broad_phase_collision_test(2000, 100, 1000, 1, true, true); broad_phase_collision_test(2000, 1000, 1000, 1, true, true); #endif } void generateEnvironments(std::vector& env, double env_scale, std::size_t n) { FCL_REAL extents[] = {-env_scale, env_scale, -env_scale, env_scale, -env_scale, env_scale}; std::vector transforms; generateRandomTransforms(extents, transforms, n); for(std::size_t i = 0; i < n; ++i) { Box* box = new Box(5, 10, 20); env.push_back(new CollisionObject(std::shared_ptr(box), transforms[i])); } generateRandomTransforms(extents, transforms, n); for(std::size_t i = 0; i < n; ++i) { Sphere* sphere = new Sphere(30); env.push_back(new CollisionObject(std::shared_ptr(sphere), transforms[i])); } generateRandomTransforms(extents, transforms, n); for(std::size_t i = 0; i < n; ++i) { Cylinder* cylinder = new Cylinder(10, 40); env.push_back(new CollisionObject(std::shared_ptr(cylinder), transforms[i])); } } void generateEnvironmentsMesh(std::vector& env, double env_scale, std::size_t n) { FCL_REAL extents[] = {-env_scale, env_scale, -env_scale, env_scale, -env_scale, env_scale}; std::vector transforms; generateRandomTransforms(extents, transforms, n); Box box(5, 10, 20); for(std::size_t i = 0; i < n; ++i) { BVHModel* model = new BVHModel(); generateBVHModel(*model, box, Transform3f()); env.push_back(new CollisionObject(std::shared_ptr(model), transforms[i])); } generateRandomTransforms(extents, transforms, n); Sphere sphere(30); for(std::size_t i = 0; i < n; ++i) { BVHModel* model = new BVHModel(); generateBVHModel(*model, sphere, Transform3f(), 16, 16); env.push_back(new CollisionObject(std::shared_ptr(model), transforms[i])); } generateRandomTransforms(extents, transforms, n); Cylinder cylinder(10, 40); for(std::size_t i = 0; i < n; ++i) { BVHModel* model = new BVHModel(); generateBVHModel(*model, cylinder, Transform3f(), 16, 16); env.push_back(new CollisionObject(std::shared_ptr(model), transforms[i])); } } void generateSelfDistanceEnvironments(std::vector& env, double env_scale, std::size_t n) { unsigned int n_edge = std::floor(std::pow(n, 1/3.0)); FCL_REAL step_size = env_scale * 2 / n_edge; FCL_REAL delta_size = step_size * 0.05; FCL_REAL single_size = step_size - 2 * delta_size; unsigned int i = 0; for(; i < n_edge * n_edge * n_edge / 4; ++i) { int x = i % (n_edge * n_edge); int y = (i - n_edge * n_edge * x) % n_edge; int z = i - n_edge * n_edge * x - n_edge * y; Box* box = new Box(single_size, single_size, single_size); env.push_back(new CollisionObject(std::shared_ptr(box), Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); } for(; i < n_edge * n_edge * n_edge / 4; ++i) { int x = i % (n_edge * n_edge); int y = (i - n_edge * n_edge * x) % n_edge; int z = i - n_edge * n_edge * x - n_edge * y; Sphere* sphere = new Sphere(single_size / 2); env.push_back(new CollisionObject(std::shared_ptr(sphere), Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); } for(; i < n_edge * n_edge * n_edge / 4; ++i) { int x = i % (n_edge * n_edge); int y = (i - n_edge * n_edge * x) % n_edge; int z = i - n_edge * n_edge * x - n_edge * y; Ellipsoid* ellipsoid = new Ellipsoid(single_size / 2, single_size / 2, single_size / 2); env.push_back(new CollisionObject(std::shared_ptr(ellipsoid), Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); } for(; i < n_edge * n_edge * n_edge / 4; ++i) { int x = i % (n_edge * n_edge); int y = (i - n_edge * n_edge * x) % n_edge; int z = i - n_edge * n_edge * x - n_edge * y; Cylinder* cylinder = new Cylinder(single_size / 2, single_size); env.push_back(new CollisionObject(std::shared_ptr(cylinder), Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); } for(; i < n_edge * n_edge * n_edge / 4; ++i) { int x = i % (n_edge * n_edge); int y = (i - n_edge * n_edge * x) % n_edge; int z = i - n_edge * n_edge * x - n_edge * y; Cone* cone = new Cone(single_size / 2, single_size); env.push_back(new CollisionObject(std::shared_ptr(cone), Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); } } void generateSelfDistanceEnvironmentsMesh(std::vector& env, double env_scale, std::size_t n) { unsigned int n_edge = std::floor(std::pow(n, 1/3.0)); FCL_REAL step_size = env_scale * 2 / n_edge; FCL_REAL delta_size = step_size * 0.05; FCL_REAL single_size = step_size - 2 * delta_size; std::size_t i = 0; for(; i < n_edge * n_edge * n_edge / 4; ++i) { int x = i % (n_edge * n_edge); int y = (i - n_edge * n_edge * x) % n_edge; int z = i - n_edge * n_edge * x - n_edge * y; Box box(single_size, single_size, single_size); BVHModel* model = new BVHModel(); generateBVHModel(*model, box, Transform3f()); env.push_back(new CollisionObject(std::shared_ptr(model), Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); } for(; i < n_edge * n_edge * n_edge / 4; ++i) { int x = i % (n_edge * n_edge); int y = (i - n_edge * n_edge * x) % n_edge; int z = i - n_edge * n_edge * x - n_edge * y; Sphere sphere(single_size / 2); BVHModel* model = new BVHModel(); generateBVHModel(*model, sphere, Transform3f(), 16, 16); env.push_back(new CollisionObject(std::shared_ptr(model), Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); } for(; i < n_edge * n_edge * n_edge / 4; ++i) { int x = i % (n_edge * n_edge); int y = (i - n_edge * n_edge * x) % n_edge; int z = i - n_edge * n_edge * x - n_edge * y; Ellipsoid ellipsoid(single_size / 2, single_size / 2, single_size / 2); BVHModel* model = new BVHModel(); generateBVHModel(*model, ellipsoid, Transform3f(), 16, 16); env.push_back(new CollisionObject(std::shared_ptr(model), Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); } for(; i < n_edge * n_edge * n_edge / 4; ++i) { int x = i % (n_edge * n_edge); int y = (i - n_edge * n_edge * x) % n_edge; int z = i - n_edge * n_edge * x - n_edge * y; Cylinder cylinder(single_size / 2, single_size); BVHModel* model = new BVHModel(); generateBVHModel(*model, cylinder, Transform3f(), 16, 16); env.push_back(new CollisionObject(std::shared_ptr(model), Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); } for(; i < n_edge * n_edge * n_edge / 4; ++i) { int x = i % (n_edge * n_edge); int y = (i - n_edge * n_edge * x) % n_edge; int z = i - n_edge * n_edge * x - n_edge * y; Cone cone(single_size / 2, single_size); BVHModel* model = new BVHModel(); generateBVHModel(*model, cone, Transform3f(), 16, 16); env.push_back(new CollisionObject(std::shared_ptr(model), Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); } } void broad_phase_collision_test(double env_scale, std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts, bool exhaustive, bool use_mesh) { std::vector ts; std::vector timers; std::vector env; if(use_mesh) generateEnvironmentsMesh(env, env_scale, env_size); else generateEnvironments(env, env_scale, env_size); std::vector query; if(use_mesh) generateEnvironmentsMesh(query, env_scale, query_size); else generateEnvironments(query, env_scale, query_size); std::vector managers; managers.push_back(new NaiveCollisionManager()); managers.push_back(new SSaPCollisionManager()); managers.push_back(new SaPCollisionManager()); managers.push_back(new IntervalTreeCollisionManager()); Vec3f lower_limit, upper_limit; SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); // FCL_REAL ncell_per_axis = std::pow((FCL_REAL)env_size / 10, 1 / 3.0); FCL_REAL ncell_per_axis = 20; FCL_REAL cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / ncell_per_axis, (upper_limit[1] - lower_limit[1]) / ncell_per_axis), (upper_limit[2] - lower_limit[2]) / ncell_per_axis); // managers.push_back(new SpatialHashingCollisionManager<>(cell_size, lower_limit, upper_limit)); managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); #if USE_GOOGLEHASH managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); #endif managers.push_back(new DynamicAABBTreeCollisionManager()); managers.push_back(new DynamicAABBTreeCollisionManager_Array()); { DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager(); m->tree_init_level = 2; managers.push_back(m); } { DynamicAABBTreeCollisionManager_Array* m = new DynamicAABBTreeCollisionManager_Array(); m->tree_init_level = 2; managers.push_back(m); } ts.resize(managers.size()); timers.resize(managers.size()); for(size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->registerObjects(env); timers[i].stop(); ts[i].push_back(timers[i].getElapsedTime()); } for(size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->setup(); timers[i].stop(); ts[i].push_back(timers[i].getElapsedTime()); } std::vector self_data(managers.size()); for(size_t i = 0; i < managers.size(); ++i) { if(exhaustive) self_data[i].request.num_max_contacts = 100000; else self_data[i].request.num_max_contacts = num_max_contacts; } for(size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->collide(&self_data[i], defaultCollisionFunction); timers[i].stop(); ts[i].push_back(timers[i].getElapsedTime()); } for(size_t i = 0; i < managers.size(); ++i) std::cout << self_data[i].result.numContacts() << " "; std::cout << std::endl; if(exhaustive) { for(size_t i = 1; i < managers.size(); ++i) BOOST_CHECK(self_data[i].result.numContacts() == self_data[0].result.numContacts()); } else { std::vector self_res(managers.size()); for(size_t i = 0; i < self_res.size(); ++i) self_res[i] = (self_data[i].result.numContacts() > 0); for(size_t i = 1; i < self_res.size(); ++i) BOOST_CHECK(self_res[0] == self_res[i]); for(size_t i = 1; i < managers.size(); ++i) BOOST_CHECK(self_data[i].result.numContacts() == self_data[0].result.numContacts()); } for(size_t i = 0; i < query.size(); ++i) { std::vector query_data(managers.size()); for(size_t j = 0; j < query_data.size(); ++j) { if(exhaustive) query_data[j].request.num_max_contacts = 100000; else query_data[j].request.num_max_contacts = num_max_contacts; } for(size_t j = 0; j < query_data.size(); ++j) { timers[j].start(); managers[j]->collide(query[i], &query_data[j], defaultCollisionFunction); timers[j].stop(); ts[j].push_back(timers[j].getElapsedTime()); } // for(size_t j = 0; j < managers.size(); ++j) // std::cout << query_data[j].result.numContacts() << " "; // std::cout << std::endl; if(exhaustive) { for(size_t j = 1; j < managers.size(); ++j) BOOST_CHECK(query_data[j].result.numContacts() == query_data[0].result.numContacts()); } else { std::vector query_res(managers.size()); for(size_t j = 0; j < query_res.size(); ++j) query_res[j] = (query_data[j].result.numContacts() > 0); for(size_t j = 1; j < query_res.size(); ++j) BOOST_CHECK(query_res[0] == query_res[j]); for(size_t j = 1; j < managers.size(); ++j) BOOST_CHECK(query_data[j].result.numContacts() == query_data[0].result.numContacts()); } } for(size_t i = 0; i < env.size(); ++i) delete env[i]; for(size_t i = 0; i < query.size(); ++i) delete query[i]; for(size_t i = 0; i < managers.size(); ++i) delete managers[i]; std::cout.setf(std::ios_base::left, std::ios_base::adjustfield); size_t w = 7; std::cout << "collision timing summary" << std::endl; std::cout << env_size << " objs, " << query_size << " queries" << std::endl; std::cout << "register time" << std::endl; for(size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[0] << " "; std::cout << std::endl; std::cout << "setup time" << std::endl; for(size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[1] << " "; std::cout << std::endl; std::cout << "self collision time" << std::endl; for(size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[2] << " "; std::cout << std::endl; std::cout << "collision time" << std::endl; for(size_t i = 0; i < ts.size(); ++i) { FCL_REAL tmp = 0; for(size_t j = 3; j < ts[i].records.size(); ++j) tmp += ts[i].records[j]; std::cout << std::setw(w) << tmp << " "; } std::cout << std::endl; std::cout << "overall time" << std::endl; for(size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].overall_time << " "; std::cout << std::endl; std::cout << std::endl; } void broad_phase_self_distance_test(double env_scale, std::size_t env_size, bool use_mesh) { std::vector ts; std::vector timers; std::vector env; if(use_mesh) generateSelfDistanceEnvironmentsMesh(env, env_scale, env_size); else generateSelfDistanceEnvironments(env, env_scale, env_size); std::vector managers; managers.push_back(new NaiveCollisionManager()); managers.push_back(new SSaPCollisionManager()); managers.push_back(new SaPCollisionManager()); managers.push_back(new IntervalTreeCollisionManager()); Vec3f lower_limit, upper_limit; SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); FCL_REAL cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 5, (upper_limit[1] - lower_limit[1]) / 5), (upper_limit[2] - lower_limit[2]) / 5); // managers.push_back(new SpatialHashingCollisionManager<>(cell_size, lower_limit, upper_limit)); managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); #if USE_GOOGLEHASH managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); #endif managers.push_back(new DynamicAABBTreeCollisionManager()); managers.push_back(new DynamicAABBTreeCollisionManager_Array()); { DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager(); m->tree_init_level = 2; managers.push_back(m); } { DynamicAABBTreeCollisionManager_Array* m = new DynamicAABBTreeCollisionManager_Array(); m->tree_init_level = 2; managers.push_back(m); } ts.resize(managers.size()); timers.resize(managers.size()); for(size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->registerObjects(env); timers[i].stop(); ts[i].push_back(timers[i].getElapsedTime()); } for(size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->setup(); timers[i].stop(); ts[i].push_back(timers[i].getElapsedTime()); } std::vector self_data(managers.size()); for(size_t i = 0; i < self_data.size(); ++i) { timers[i].start(); managers[i]->distance(&self_data[i], defaultDistanceFunction); timers[i].stop(); ts[i].push_back(timers[i].getElapsedTime()); // std::cout << self_data[i].result.min_distance << " "; } // std::cout << std::endl; for(size_t i = 1; i < managers.size(); ++i) BOOST_CHECK(fabs(self_data[0].result.min_distance - self_data[i].result.min_distance) < DELTA || fabs(self_data[0].result.min_distance - self_data[i].result.min_distance) / fabs(self_data[0].result.min_distance) < DELTA); for(size_t i = 0; i < env.size(); ++i) delete env[i]; for(size_t i = 0; i < managers.size(); ++i) delete managers[i]; std::cout.setf(std::ios_base::left, std::ios_base::adjustfield); size_t w = 7; std::cout << "self distance timing summary" << std::endl; std::cout << env.size() << " objs" << std::endl; std::cout << "register time" << std::endl; for(size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[0] << " "; std::cout << std::endl; std::cout << "setup time" << std::endl; for(size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[1] << " "; std::cout << std::endl; std::cout << "self distance time" << std::endl; for(size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[2] << " "; std::cout << std::endl; std::cout << "overall time" << std::endl; for(size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].overall_time << " "; std::cout << std::endl; std::cout << std::endl; } void broad_phase_distance_test(double env_scale, std::size_t env_size, std::size_t query_size, bool use_mesh) { std::vector ts; std::vector timers; std::vector env; if(use_mesh) generateEnvironmentsMesh(env, env_scale, env_size); else generateEnvironments(env, env_scale, env_size); std::vector query; BroadPhaseCollisionManager* manager = new NaiveCollisionManager(); for(std::size_t i = 0; i < env.size(); ++i) manager->registerObject(env[i]); manager->setup(); while(1) { std::vector candidates; if(use_mesh) generateEnvironmentsMesh(candidates, env_scale, query_size); else generateEnvironments(candidates, env_scale, query_size); for(std::size_t i = 0; i < candidates.size(); ++i) { CollisionData query_data; manager->collide(candidates[i], &query_data, defaultCollisionFunction); if(query_data.result.numContacts() == 0) query.push_back(candidates[i]); else delete candidates[i]; if(query.size() == query_size) break; } if(query.size() == query_size) break; } delete manager; std::vector managers; managers.push_back(new NaiveCollisionManager()); managers.push_back(new SSaPCollisionManager()); managers.push_back(new SaPCollisionManager()); managers.push_back(new IntervalTreeCollisionManager()); Vec3f lower_limit, upper_limit; SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); FCL_REAL cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 20, (upper_limit[1] - lower_limit[1]) / 20), (upper_limit[2] - lower_limit[2])/20); // managers.push_back(new SpatialHashingCollisionManager<>(cell_size, lower_limit, upper_limit)); managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); #if USE_GOOGLEHASH managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); #endif managers.push_back(new DynamicAABBTreeCollisionManager()); managers.push_back(new DynamicAABBTreeCollisionManager_Array()); { DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager(); m->tree_init_level = 2; managers.push_back(m); } { DynamicAABBTreeCollisionManager_Array* m = new DynamicAABBTreeCollisionManager_Array(); m->tree_init_level = 2; managers.push_back(m); } ts.resize(managers.size()); timers.resize(managers.size()); for(size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->registerObjects(env); timers[i].stop(); ts[i].push_back(timers[i].getElapsedTime()); } for(size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->setup(); timers[i].stop(); ts[i].push_back(timers[i].getElapsedTime()); } for(size_t i = 0; i < query.size(); ++i) { std::vector query_data(managers.size()); for(size_t j = 0; j < managers.size(); ++j) { timers[j].start(); managers[j]->distance(query[i], &query_data[j], defaultDistanceFunction); timers[j].stop(); ts[j].push_back(timers[j].getElapsedTime()); // std::cout << query_data[j].result.min_distance << " "; } // std::cout << std::endl; for(size_t j = 1; j < managers.size(); ++j) BOOST_CHECK(fabs(query_data[0].result.min_distance - query_data[j].result.min_distance) < DELTA || fabs(query_data[0].result.min_distance - query_data[j].result.min_distance) / fabs(query_data[0].result.min_distance) < DELTA); } for(std::size_t i = 0; i < env.size(); ++i) delete env[i]; for(std::size_t i = 0; i < query.size(); ++i) delete query[i]; for(size_t i = 0; i < managers.size(); ++i) delete managers[i]; std::cout.setf(std::ios_base::left, std::ios_base::adjustfield); size_t w = 7; std::cout << "distance timing summary" << std::endl; std::cout << env_size << " objs, " << query_size << " queries" << std::endl; std::cout << "register time" << std::endl; for(size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[0] << " "; std::cout << std::endl; std::cout << "setup time" << std::endl; for(size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[1] << " "; std::cout << std::endl; std::cout << "distance time" << std::endl; for(size_t i = 0; i < ts.size(); ++i) { FCL_REAL tmp = 0; for(size_t j = 2; j < ts[i].records.size(); ++j) tmp += ts[i].records[j]; std::cout << std::setw(w) << tmp << " "; } std::cout << std::endl; std::cout << "overall time" << std::endl; for(size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].overall_time << " "; std::cout << std::endl; std::cout << std::endl; } void broad_phase_update_collision_test(double env_scale, std::size_t env_size, std::size_t query_size, std::size_t num_max_contacts, bool exhaustive, bool use_mesh) { std::vector ts; std::vector timers; std::vector env; if(use_mesh) generateEnvironmentsMesh(env, env_scale, env_size); else generateEnvironments(env, env_scale, env_size); std::vector query; if(use_mesh) generateEnvironmentsMesh(query, env_scale, query_size); else generateEnvironments(query, env_scale, query_size); std::vector managers; managers.push_back(new NaiveCollisionManager()); managers.push_back(new SSaPCollisionManager()); managers.push_back(new SaPCollisionManager()); managers.push_back(new IntervalTreeCollisionManager()); Vec3f lower_limit, upper_limit; SpatialHashingCollisionManager<>::computeBound(env, lower_limit, upper_limit); FCL_REAL cell_size = std::min(std::min((upper_limit[0] - lower_limit[0]) / 20, (upper_limit[1] - lower_limit[1]) / 20), (upper_limit[2] - lower_limit[2])/20); // managers.push_back(new SpatialHashingCollisionManager<>(cell_size, lower_limit, upper_limit)); managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); #if USE_GOOGLEHASH managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); managers.push_back(new SpatialHashingCollisionManager >(cell_size, lower_limit, upper_limit)); #endif managers.push_back(new DynamicAABBTreeCollisionManager()); managers.push_back(new DynamicAABBTreeCollisionManager_Array()); { DynamicAABBTreeCollisionManager* m = new DynamicAABBTreeCollisionManager(); m->tree_init_level = 2; managers.push_back(m); } { DynamicAABBTreeCollisionManager_Array* m = new DynamicAABBTreeCollisionManager_Array(); m->tree_init_level = 2; managers.push_back(m); } ts.resize(managers.size()); timers.resize(managers.size()); for(size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->registerObjects(env); timers[i].stop(); ts[i].push_back(timers[i].getElapsedTime()); } for(size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->setup(); timers[i].stop(); ts[i].push_back(timers[i].getElapsedTime()); } // update the environment FCL_REAL delta_angle_max = 10 / 360.0 * 2 * constants::pi; FCL_REAL delta_trans_max = 0.01 * env_scale; for(size_t i = 0; i < env.size(); ++i) { FCL_REAL rand_angle_x = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; FCL_REAL rand_trans_x = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; FCL_REAL rand_angle_y = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; FCL_REAL rand_trans_y = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; FCL_REAL rand_angle_z = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_angle_max; FCL_REAL rand_trans_z = 2 * (rand() / (FCL_REAL)RAND_MAX - 0.5) * delta_trans_max; Quaternion3f q1, q2, q3; q1.fromAxisAngle(Vec3f(1, 0, 0), rand_angle_x); q2.fromAxisAngle(Vec3f(0, 1, 0), rand_angle_y); q3.fromAxisAngle(Vec3f(0, 0, 1), rand_angle_z); Quaternion3f q = q1 * q2 * q3; Matrix3f dR; q.toRotation(dR); Vec3f dT(rand_trans_x, rand_trans_y, rand_trans_z); Matrix3f R = env[i]->getRotation(); Vec3f T = env[i]->getTranslation(); env[i]->setTransform(dR * R, dR * T + dT); env[i]->computeAABB(); } for(size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->update(); timers[i].stop(); ts[i].push_back(timers[i].getElapsedTime()); } std::vector self_data(managers.size()); for(size_t i = 0; i < managers.size(); ++i) { if(exhaustive) self_data[i].request.num_max_contacts = 100000; else self_data[i].request.num_max_contacts = num_max_contacts; } for(size_t i = 0; i < managers.size(); ++i) { timers[i].start(); managers[i]->collide(&self_data[i], defaultCollisionFunction); timers[i].stop(); ts[i].push_back(timers[i].getElapsedTime()); } for(size_t i = 0; i < managers.size(); ++i) std::cout << self_data[i].result.numContacts() << " "; std::cout << std::endl; if(exhaustive) { for(size_t i = 1; i < managers.size(); ++i) BOOST_CHECK(self_data[i].result.numContacts() == self_data[0].result.numContacts()); } else { std::vector self_res(managers.size()); for(size_t i = 0; i < self_res.size(); ++i) self_res[i] = (self_data[i].result.numContacts() > 0); for(size_t i = 1; i < self_res.size(); ++i) BOOST_CHECK(self_res[0] == self_res[i]); for(size_t i = 1; i < managers.size(); ++i) BOOST_CHECK(self_data[i].result.numContacts() == self_data[0].result.numContacts()); } for(size_t i = 0; i < query.size(); ++i) { std::vector query_data(managers.size()); for(size_t j = 0; j < query_data.size(); ++j) { if(exhaustive) query_data[j].request.num_max_contacts = 100000; else query_data[j].request.num_max_contacts = num_max_contacts; } for(size_t j = 0; j < query_data.size(); ++j) { timers[j].start(); managers[j]->collide(query[i], &query_data[j], defaultCollisionFunction); timers[j].stop(); ts[j].push_back(timers[j].getElapsedTime()); } // for(size_t j = 0; j < managers.size(); ++j) // std::cout << query_data[j].result.numContacts() << " "; // std::cout << std::endl; if(exhaustive) { for(size_t j = 1; j < managers.size(); ++j) BOOST_CHECK(query_data[j].result.numContacts() == query_data[0].result.numContacts()); } else { std::vector query_res(managers.size()); for(size_t j = 0; j < query_res.size(); ++j) query_res[j] = (query_data[j].result.numContacts() > 0); for(size_t j = 1; j < query_res.size(); ++j) BOOST_CHECK(query_res[0] == query_res[j]); for(size_t j = 1; j < managers.size(); ++j) BOOST_CHECK(query_data[j].result.numContacts() == query_data[0].result.numContacts()); } } for(size_t i = 0; i < env.size(); ++i) delete env[i]; for(size_t i = 0; i < query.size(); ++i) delete query[i]; for(size_t i = 0; i < managers.size(); ++i) delete managers[i]; std::cout.setf(std::ios_base::left, std::ios_base::adjustfield); size_t w = 7; std::cout << "collision timing summary" << std::endl; std::cout << env_size << " objs, " << query_size << " queries" << std::endl; std::cout << "register time" << std::endl; for(size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[0] << " "; std::cout << std::endl; std::cout << "setup time" << std::endl; for(size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[1] << " "; std::cout << std::endl; std::cout << "update time" << std::endl; for(size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[2] << " "; std::cout << std::endl; std::cout << "self collision time" << std::endl; for(size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].records[3] << " "; std::cout << std::endl; std::cout << "collision time" << std::endl; for(size_t i = 0; i < ts.size(); ++i) { FCL_REAL tmp = 0; for(size_t j = 4; j < ts[i].records.size(); ++j) tmp += ts[i].records[j]; std::cout << std::setw(w) << tmp << " "; } std::cout << std::endl; std::cout << "overall time" << std::endl; for(size_t i = 0; i < ts.size(); ++i) std::cout << std::setw(w) << ts[i].overall_time << " "; std::cout << std::endl; std::cout << std::endl; } fcl-0.5.0/test/test_fcl_bvh_models.cpp000066400000000000000000000155261274356571000177730ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jeongseok Lee */ #define BOOST_TEST_MODULE "FCL_BVH_MODELS" #include #include "fcl/config.h" #include "fcl/BVH/BVH_model.h" #include "fcl/math/transform.h" #include "fcl/shape/geometric_shapes.h" #include "test_fcl_utility.h" #include using namespace fcl; template void testBVHModelPointCloud() { std::shared_ptr > model(new BVHModel); if (model->getNodeType() != BV_AABB && model->getNodeType() != BV_KDOP16 && model->getNodeType() != BV_KDOP18 && model->getNodeType() != BV_KDOP24) { std::cout << "Abort test since '" << getNodeTypeName(model->getNodeType()) << "' does not support point cloud model. " << "Please see issue #67." << std::endl; return; } Box box; double a = box.side[0]; double b = box.side[1]; double c = box.side[2]; std::vector points(8); points[0].setValue(0.5 * a, -0.5 * b, 0.5 * c); points[1].setValue(0.5 * a, 0.5 * b, 0.5 * c); points[2].setValue(-0.5 * a, 0.5 * b, 0.5 * c); points[3].setValue(-0.5 * a, -0.5 * b, 0.5 * c); points[4].setValue(0.5 * a, -0.5 * b, -0.5 * c); points[5].setValue(0.5 * a, 0.5 * b, -0.5 * c); points[6].setValue(-0.5 * a, 0.5 * b, -0.5 * c); points[7].setValue(-0.5 * a, -0.5 * b, -0.5 * c); int result; result = model->beginModel(); BOOST_CHECK_EQUAL(result, BVH_OK); for (std::size_t i = 0; i < points.size(); ++i) { result = model->addVertex(points[i]); BOOST_CHECK_EQUAL(result, BVH_OK); } result = model->endModel(); BOOST_CHECK_EQUAL(result, BVH_OK); model->computeLocalAABB(); BOOST_CHECK_EQUAL(model->num_vertices, 8); BOOST_CHECK_EQUAL(model->num_tris, 0); BOOST_CHECK_EQUAL(model->build_state, BVH_BUILD_STATE_PROCESSED); } template void testBVHModelTriangles() { std::shared_ptr > model(new BVHModel); Box box; double a = box.side[0]; double b = box.side[1]; double c = box.side[2]; std::vector points(8); std::vector tri_indices(12); points[0].setValue(0.5 * a, -0.5 * b, 0.5 * c); points[1].setValue(0.5 * a, 0.5 * b, 0.5 * c); points[2].setValue(-0.5 * a, 0.5 * b, 0.5 * c); points[3].setValue(-0.5 * a, -0.5 * b, 0.5 * c); points[4].setValue(0.5 * a, -0.5 * b, -0.5 * c); points[5].setValue(0.5 * a, 0.5 * b, -0.5 * c); points[6].setValue(-0.5 * a, 0.5 * b, -0.5 * c); points[7].setValue(-0.5 * a, -0.5 * b, -0.5 * c); tri_indices[0].set(0, 4, 1); tri_indices[1].set(1, 4, 5); tri_indices[2].set(2, 6, 3); tri_indices[3].set(3, 6, 7); tri_indices[4].set(3, 0, 2); tri_indices[5].set(2, 0, 1); tri_indices[6].set(6, 5, 7); tri_indices[7].set(7, 5, 4); tri_indices[8].set(1, 5, 2); tri_indices[9].set(2, 5, 6); tri_indices[10].set(3, 7, 0); tri_indices[11].set(0, 7, 4); int result; result = model->beginModel(); BOOST_CHECK_EQUAL(result, BVH_OK); for (std::size_t i = 0; i < tri_indices.size(); ++i) { result = model->addTriangle(points[tri_indices[i][0]], points[tri_indices[i][1]], points[tri_indices[i][2]]); BOOST_CHECK_EQUAL(result, BVH_OK); } result = model->endModel(); BOOST_CHECK_EQUAL(result, BVH_OK); model->computeLocalAABB(); BOOST_CHECK_EQUAL(model->num_vertices, 12 * 3); BOOST_CHECK_EQUAL(model->num_tris, 12); BOOST_CHECK_EQUAL(model->build_state, BVH_BUILD_STATE_PROCESSED); } template void testBVHModelSubModel() { std::shared_ptr > model(new BVHModel); Box box; double a = box.side[0]; double b = box.side[1]; double c = box.side[2]; std::vector points(8); std::vector tri_indices(12); points[0].setValue(0.5 * a, -0.5 * b, 0.5 * c); points[1].setValue(0.5 * a, 0.5 * b, 0.5 * c); points[2].setValue(-0.5 * a, 0.5 * b, 0.5 * c); points[3].setValue(-0.5 * a, -0.5 * b, 0.5 * c); points[4].setValue(0.5 * a, -0.5 * b, -0.5 * c); points[5].setValue(0.5 * a, 0.5 * b, -0.5 * c); points[6].setValue(-0.5 * a, 0.5 * b, -0.5 * c); points[7].setValue(-0.5 * a, -0.5 * b, -0.5 * c); tri_indices[0].set(0, 4, 1); tri_indices[1].set(1, 4, 5); tri_indices[2].set(2, 6, 3); tri_indices[3].set(3, 6, 7); tri_indices[4].set(3, 0, 2); tri_indices[5].set(2, 0, 1); tri_indices[6].set(6, 5, 7); tri_indices[7].set(7, 5, 4); tri_indices[8].set(1, 5, 2); tri_indices[9].set(2, 5, 6); tri_indices[10].set(3, 7, 0); tri_indices[11].set(0, 7, 4); int result; result = model->beginModel(); BOOST_CHECK_EQUAL(result, BVH_OK); result = model->addSubModel(points, tri_indices); BOOST_CHECK_EQUAL(result, BVH_OK); result = model->endModel(); BOOST_CHECK_EQUAL(result, BVH_OK); model->computeLocalAABB(); BOOST_CHECK_EQUAL(model->num_vertices, 8); BOOST_CHECK_EQUAL(model->num_tris, 12); BOOST_CHECK_EQUAL(model->build_state, BVH_BUILD_STATE_PROCESSED); } template void testBVHModel() { testBVHModelTriangles(); testBVHModelPointCloud(); testBVHModelSubModel(); } BOOST_AUTO_TEST_CASE(building_bvh_models) { testBVHModel(); testBVHModel(); testBVHModel(); testBVHModel(); testBVHModel(); testBVHModel >(); testBVHModel >(); testBVHModel >(); } fcl-0.5.0/test/test_fcl_capsule_box_1.cpp000066400000000000000000000106271274356571000203720ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2014-2016, CNRS-LAAS and AIST * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of CNRS-LAAS and AIST 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 OWNER 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. */ /** \author Florent Lamiraux */ #define BOOST_TEST_MODULE "FCL_GEOMETRIC_SHAPES" #define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE ((x + 1.0), (1.0), (eps)) #include #include #include #include #include #include #include BOOST_AUTO_TEST_CASE(distance_capsule_box) { typedef std::shared_ptr CollisionGeometryPtr_t; // Capsule of radius 2 and of height 4 CollisionGeometryPtr_t capsuleGeometry (new fcl::Capsule (2., 4.)); // Box of size 1 by 2 by 4 CollisionGeometryPtr_t boxGeometry (new fcl::Box (1., 2., 4.)); // Enable computation of nearest points fcl::DistanceRequest distanceRequest (true); fcl::DistanceResult distanceResult; fcl::Transform3f tf1 (fcl::Vec3f (3., 0, 0)); fcl::Transform3f tf2; fcl::CollisionObject capsule (capsuleGeometry, tf1); fcl::CollisionObject box (boxGeometry, tf2); // test distance fcl::distance (&capsule, &box, distanceRequest, distanceResult); // Nearest point on capsule fcl::Vec3f o1 (distanceResult.nearest_points [0]); // Nearest point on box fcl::Vec3f o2 (distanceResult.nearest_points [1]); BOOST_CHECK_CLOSE (distanceResult.min_distance, 0.5, 1e-4); BOOST_CHECK_CLOSE (o1 [0], -2.0, 1e-4); CHECK_CLOSE_TO_0 (o1 [1], 1e-4); BOOST_CHECK_CLOSE (o2 [0], 0.5, 1e-4); BOOST_CHECK_CLOSE (o1 [1], 0.0, 1e-4); // Move capsule above box tf1 = fcl::Transform3f (fcl::Vec3f (0., 0., 8.)); capsule.setTransform (tf1); // test distance distanceResult.clear (); fcl::distance (&capsule, &box, distanceRequest, distanceResult); o1 = distanceResult.nearest_points [0]; o2 = distanceResult.nearest_points [1]; BOOST_CHECK_CLOSE (distanceResult.min_distance, 2.0, 1e-4); CHECK_CLOSE_TO_0 (o1 [0], 1e-4); CHECK_CLOSE_TO_0 (o1 [1], 1e-4); BOOST_CHECK_CLOSE (o1 [2], -4.0, 1e-4); // Disabled broken test lines. Please see #25. // CHECK_CLOSE_TO_0 (o2 [0], 1e-4); CHECK_CLOSE_TO_0 (o2 [1], 1e-4); BOOST_CHECK_CLOSE (o2 [2], 2.0, 1e-4); // Rotate capsule around y axis by pi/2 and move it behind box tf1.setTranslation (fcl::Vec3f (-10., 0., 0.)); tf1.setQuatRotation (fcl::Quaternion3f (sqrt(2)/2, 0, sqrt(2)/2, 0)); capsule.setTransform (tf1); // test distance distanceResult.clear (); fcl::distance (&capsule, &box, distanceRequest, distanceResult); o1 = distanceResult.nearest_points [0]; o2 = distanceResult.nearest_points [1]; BOOST_CHECK_CLOSE (distanceResult.min_distance, 5.5, 1e-4); CHECK_CLOSE_TO_0 (o1 [0], 1e-4); CHECK_CLOSE_TO_0 (o1 [1], 1e-4); BOOST_CHECK_CLOSE (o1 [2], 4.0, 1e-4); BOOST_CHECK_CLOSE (o2 [0], -0.5, 1e-4); CHECK_CLOSE_TO_0 (o2 [1], 1e-4); CHECK_CLOSE_TO_0 (o2 [2], 1e-4); } fcl-0.5.0/test/test_fcl_capsule_box_2.cpp000066400000000000000000000065441274356571000203760ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2014-2016, CNRS-LAAS and AIST * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of CNRS-LAAS and AIST 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 OWNER 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. */ /** \author Florent Lamiraux */ #define BOOST_TEST_MODULE "FCL_GEOMETRIC_SHAPES" #define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE ((x + 1.0), (1.0), (eps)) #include #include #include #include #include #include #include BOOST_AUTO_TEST_CASE(distance_capsule_box) { typedef std::shared_ptr CollisionGeometryPtr_t; // Capsule of radius 2 and of height 4 CollisionGeometryPtr_t capsuleGeometry (new fcl::Capsule (2., 4.)); // Box of size 1 by 2 by 4 CollisionGeometryPtr_t boxGeometry (new fcl::Box (1., 2., 4.)); // Enable computation of nearest points fcl::DistanceRequest distanceRequest (true); fcl::DistanceResult distanceResult; // Rotate capsule around y axis by pi/2 and move it behind box fcl::Transform3f tf1 (fcl::Quaternion3f (sqrt(2)/2, 0, sqrt(2)/2, 0), fcl::Vec3f (-10., 0.8, 1.5)); fcl::Transform3f tf2; fcl::CollisionObject capsule (capsuleGeometry, tf1); fcl::CollisionObject box (boxGeometry, tf2); // test distance distanceResult.clear (); fcl::distance (&capsule, &box, distanceRequest, distanceResult); fcl::Vec3f o1 = distanceResult.nearest_points [0]; fcl::Vec3f o2 = distanceResult.nearest_points [1]; BOOST_CHECK_CLOSE (distanceResult.min_distance, 5.5, 1e-4); // Disabled broken test lines. Please see #25. // CHECK_CLOSE_TO_0 (o1 [0], 1e-4); // CHECK_CLOSE_TO_0 (o1 [1], 1e-4); BOOST_CHECK_CLOSE (o1 [2], 4.0, 1e-4); BOOST_CHECK_CLOSE (o2 [0], -0.5, 1e-4); // Disabled broken test lines. Please see #25. // BOOST_CHECK_CLOSE (o2 [1], 0.8, 1e-4); // BOOST_CHECK_CLOSE (o2 [2], 1.5, 1e-4); } fcl-0.5.0/test/test_fcl_capsule_capsule.cpp000066400000000000000000000123031274356571000210070ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Karsten Knese */ #define BOOST_TEST_MODULE "FCL_CAPSULE_CAPSULE" #include #include "fcl/math/constants.h" #include "fcl/collision.h" #include "fcl/shape/geometric_shapes.h" #include "fcl/narrowphase/narrowphase.h" #include using namespace fcl; BOOST_AUTO_TEST_CASE(distance_capsulecapsule_origin) { GJKSolver_indep solver; Capsule s1(5, 10); Capsule s2(5, 10); Vec3f closest_p1, closest_p2; Transform3f transform; Transform3f transform2; transform2 = Transform3f(Vec3f(20.1, 0,0)); bool res; FCL_REAL dist; res = solver.shapeDistance(s1, transform, s2, transform2, &dist, &closest_p1, &closest_p2); std::cerr << "applied transformation of two caps: " << transform.getTranslation() << " & " << transform2.getTranslation() << std::endl; std::cerr << "computed points in caps to caps" << closest_p1 << " & " << closest_p2 << "with dist: " << dist << std::endl; BOOST_CHECK(std::abs(dist - 10.1) < 0.001); BOOST_CHECK(res); } BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformXY) { GJKSolver_indep solver; Capsule s1(5, 10); Capsule s2(5, 10); Vec3f closest_p1, closest_p2; Transform3f transform; Transform3f transform2; transform2 = Transform3f(Vec3f(20, 20,0)); bool res; FCL_REAL dist; res = solver.shapeDistance(s1, transform, s2, transform2, &dist, &closest_p1, &closest_p2); std::cerr << "applied transformation of two caps: " << transform.getTranslation() << " & " << transform2.getTranslation() << std::endl; std::cerr << "computed points in caps to caps" << closest_p1 << " & " << closest_p2 << "with dist: " << dist << std::endl; FCL_REAL expected = std::sqrt(FCL_REAL(800)) - 10; BOOST_CHECK(std::abs(expected-dist) < 0.01); BOOST_CHECK(res); } BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ) { GJKSolver_indep solver; Capsule s1(5, 10); Capsule s2(5, 10); Vec3f closest_p1, closest_p2; Transform3f transform; Transform3f transform2; transform2 = Transform3f(Vec3f(0,0,20.1)); bool res; FCL_REAL dist; res = solver.shapeDistance(s1, transform, s2, transform2, &dist, &closest_p1, &closest_p2); std::cerr << "applied transformation of two caps: " << transform.getTranslation() << " & " << transform2.getTranslation() << std::endl; std::cerr << "computed points in caps to caps" << closest_p1 << " & " << closest_p2 << "with dist: " << dist << std::endl; BOOST_CHECK(std::abs(dist - 0.1) < 0.001); BOOST_CHECK(res); } BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ2) { const FCL_REAL Pi = constants::pi; GJKSolver_indep solver; Capsule s1(5, 10); Capsule s2(5, 10); Vec3f closest_p1, closest_p2; Transform3f transform; Transform3f transform2; transform2 = Transform3f(Vec3f(0,0,25.1)); Matrix3f rot2; rot2.setEulerZYX(0,Pi/2,0); transform2.setRotation(rot2); bool res; FCL_REAL dist; res = solver.shapeDistance(s1, transform, s2, transform2, &dist, &closest_p1, &closest_p2); std::cerr << "applied transformation of two caps: " << transform.getTranslation() << " & " << transform2.getTranslation() << std::endl; std::cerr << "applied transformation of two caps: " << transform.getRotation() << " & " << transform2.getRotation() << std::endl; std::cerr << "computed points in caps to caps" << closest_p1 << " & " << closest_p2 << "with dist: " << dist << std::endl; BOOST_CHECK(std::abs(dist - 5.1) < 0.001); BOOST_CHECK(res); } fcl-0.5.0/test/test_fcl_collision.cpp000066400000000000000000001161111274356571000176340ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #define BOOST_TEST_MODULE "FCL_COLLISION" #include #include "fcl/traversal/traversal_node_bvhs.h" #include "fcl/traversal/traversal_node_setup.h" #include "fcl/collision_node.h" #include "fcl/collision.h" #include "fcl/BV/BV.h" #include "fcl/shape/geometric_shapes.h" #include "fcl/narrowphase/narrowphase.h" #include "test_fcl_utility.h" #include "fcl_resources/config.h" #include using namespace fcl; template bool collide_Test(const Transform3f& tf, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose = true); template bool collide_Test2(const Transform3f& tf, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose = true); template bool collide_Test_Oriented(const Transform3f& tf, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose = true); template bool test_collide_func(const Transform3f& tf, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method); int num_max_contacts = std::numeric_limits::max(); bool enable_contact = true; std::vector global_pairs; std::vector global_pairs_now; BOOST_AUTO_TEST_CASE(OBB_Box_test) { FCL_REAL r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; std::vector rotate_transform; generateRandomTransforms(r_extents, rotate_transform, 1); AABB aabb1; aabb1.min_ = Vec3f(-600, -600, -600); aabb1.max_ = Vec3f(600, 600, 600); OBB obb1; convertBV(aabb1, rotate_transform[0], obb1); Box box1; Transform3f box1_tf; constructBox(aabb1, rotate_transform[0], box1, box1_tf); FCL_REAL extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; std::size_t n = 1000; std::vector transforms; generateRandomTransforms(extents, transforms, n); for(std::size_t i = 0; i < transforms.size(); ++i) { AABB aabb; aabb.min_ = aabb1.min_ * 0.5; aabb.max_ = aabb1.max_ * 0.5; OBB obb2; convertBV(aabb, transforms[i], obb2); Box box2; Transform3f box2_tf; constructBox(aabb, transforms[i], box2, box2_tf); GJKSolver_libccd solver; bool overlap_obb = obb1.overlap(obb2); bool overlap_box = solver.shapeIntersect(box1, box1_tf, box2, box2_tf, NULL); BOOST_CHECK(overlap_obb == overlap_box); } } BOOST_AUTO_TEST_CASE(OBB_shape_test) { FCL_REAL r_extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; std::vector rotate_transform; generateRandomTransforms(r_extents, rotate_transform, 1); AABB aabb1; aabb1.min_ = Vec3f(-600, -600, -600); aabb1.max_ = Vec3f(600, 600, 600); OBB obb1; convertBV(aabb1, rotate_transform[0], obb1); Box box1; Transform3f box1_tf; constructBox(aabb1, rotate_transform[0], box1, box1_tf); FCL_REAL extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; std::size_t n = 1000; std::vector transforms; generateRandomTransforms(extents, transforms, n); for(std::size_t i = 0; i < transforms.size(); ++i) { FCL_REAL len = (aabb1.max_[0] - aabb1.min_[0]) * 0.5; OBB obb2; GJKSolver_libccd solver; { Sphere sphere(len); computeBV(sphere, transforms[i], obb2); bool overlap_obb = obb1.overlap(obb2); bool overlap_sphere = solver.shapeIntersect(box1, box1_tf, sphere, transforms[i], NULL); BOOST_CHECK(overlap_obb >= overlap_sphere); } { Ellipsoid ellipsoid(len, len, len); computeBV(ellipsoid, transforms[i], obb2); bool overlap_obb = obb1.overlap(obb2); bool overlap_ellipsoid = solver.shapeIntersect(box1, box1_tf, ellipsoid, transforms[i], NULL); BOOST_CHECK(overlap_obb >= overlap_ellipsoid); } { Capsule capsule(len, 2 * len); computeBV(capsule, transforms[i], obb2); bool overlap_obb = obb1.overlap(obb2); bool overlap_capsule = solver.shapeIntersect(box1, box1_tf, capsule, transforms[i], NULL); BOOST_CHECK(overlap_obb >= overlap_capsule); } { Cone cone(len, 2 * len); computeBV(cone, transforms[i], obb2); bool overlap_obb = obb1.overlap(obb2); bool overlap_cone = solver.shapeIntersect(box1, box1_tf, cone, transforms[i], NULL); BOOST_CHECK(overlap_obb >= overlap_cone); } { Cylinder cylinder(len, 2 * len); computeBV(cylinder, transforms[i], obb2); bool overlap_obb = obb1.overlap(obb2); bool overlap_cylinder = solver.shapeIntersect(box1, box1_tf, cylinder, transforms[i], NULL); BOOST_CHECK(overlap_obb >= overlap_cylinder); } } } BOOST_AUTO_TEST_CASE(OBB_AABB_test) { FCL_REAL extents[] = {-1000, -1000, -1000, 1000, 1000, 1000}; std::size_t n = 1000; std::vector transforms; generateRandomTransforms(extents, transforms, n); AABB aabb1; aabb1.min_ = Vec3f(-600, -600, -600); aabb1.max_ = Vec3f(600, 600, 600); OBB obb1; convertBV(aabb1, Transform3f(), obb1); for(std::size_t i = 0; i < transforms.size(); ++i) { AABB aabb; aabb.min_ = aabb1.min_ * 0.5; aabb.max_ = aabb1.max_ * 0.5; AABB aabb2 = translate(aabb, transforms[i].getTranslation()); OBB obb2; convertBV(aabb, Transform3f(transforms[i].getTranslation()), obb2); bool overlap_aabb = aabb1.overlap(aabb2); bool overlap_obb = obb1.overlap(obb2); if(overlap_aabb != overlap_obb) { std::cout << aabb1.min_ << " " << aabb1.max_ << std::endl; std::cout << aabb2.min_ << " " << aabb2.max_ << std::endl; std::cout << obb1.To << " " << obb1.extent << " " << obb1.axis[0] << " " << obb1.axis[1] << " " << obb1.axis[2] << std::endl; std::cout << obb2.To << " " << obb2.extent << " " << obb2.axis[0] << " " << obb2.axis[1] << " " << obb2.axis[2] << std::endl; } BOOST_CHECK(overlap_aabb == overlap_obb); } std::cout << std::endl; } BOOST_AUTO_TEST_CASE(mesh_mesh) { std::vector p1, p2; std::vector t1, t2; boost::filesystem::path path(TEST_RESOURCES_DIR); loadOBJFile((path / "env.obj").string().c_str(), p1, t1); loadOBJFile((path / "rob.obj").string().c_str(), p2, t2); std::vector transforms; FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; std::size_t n = 10; bool verbose = false; generateRandomTransforms(extents, transforms, n); // collision for(std::size_t i = 0; i < transforms.size(); ++i) { global_pairs.clear(); global_pairs_now.clear(); collide_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); collide_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test2(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test2(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test2(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test2(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test2(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test2(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test2(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test2(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test2(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test2 >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test2 >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test2 >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test2 >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test2 >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test2 >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test2 >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test2 >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test2 >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } test_collide_func(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } test_collide_func(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } test_collide_func(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test2(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test2(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test2(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } test_collide_func(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } test_collide_func(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } test_collide_func(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test2(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test2(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test2(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } collide_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } test_collide_func(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } test_collide_func(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } test_collide_func(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER); BOOST_CHECK(global_pairs.size() == global_pairs_now.size()); for(std::size_t j = 0; j < global_pairs.size(); ++j) { BOOST_CHECK(global_pairs[j].b1 == global_pairs_now[j].b1); BOOST_CHECK(global_pairs[j].b2 == global_pairs_now[j].b2); } } } template bool collide_Test2(const Transform3f& tf, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose) { BVHModel m1; BVHModel m2; m1.bv_splitter.reset(new BVSplitter(split_method)); m2.bv_splitter.reset(new BVSplitter(split_method)); std::vector vertices1_new(vertices1.size()); for(unsigned int i = 0; i < vertices1_new.size(); ++i) { vertices1_new[i] = tf.transform(vertices1[i]); } m1.beginModel(); m1.addSubModel(vertices1_new, triangles1); m1.endModel(); m2.beginModel(); m2.addSubModel(vertices2, triangles2); m2.endModel(); Transform3f pose1, pose2; CollisionResult local_result; MeshCollisionTraversalNode node; if(!initialize(node, m1, pose1, m2, pose2, CollisionRequest(num_max_contacts, enable_contact), local_result)) std::cout << "initialize error" << std::endl; node.enable_statistics = verbose; collide(&node); if(local_result.numContacts() > 0) { if(global_pairs.size() == 0) { local_result.getContacts(global_pairs); std::sort(global_pairs.begin(), global_pairs.end()); } else { local_result.getContacts(global_pairs_now); std::sort(global_pairs_now.begin(), global_pairs_now.end()); } if(verbose) std::cout << "in collision " << local_result.numContacts() << ": " << std::endl; if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl; return true; } else { if(verbose) std::cout << "collision free " << std::endl; if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl; return false; } } template bool collide_Test(const Transform3f& tf, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose) { BVHModel m1; BVHModel m2; m1.bv_splitter.reset(new BVSplitter(split_method)); m2.bv_splitter.reset(new BVSplitter(split_method)); m1.beginModel(); m1.addSubModel(vertices1, triangles1); m1.endModel(); m2.beginModel(); m2.addSubModel(vertices2, triangles2); m2.endModel(); Transform3f pose1(tf), pose2; CollisionResult local_result; MeshCollisionTraversalNode node; if(!initialize(node, m1, pose1, m2, pose2, CollisionRequest(num_max_contacts, enable_contact), local_result)) std::cout << "initialize error" << std::endl; node.enable_statistics = verbose; collide(&node); if(local_result.numContacts() > 0) { if(global_pairs.size() == 0) { local_result.getContacts(global_pairs); std::sort(global_pairs.begin(), global_pairs.end()); } else { local_result.getContacts(global_pairs_now); std::sort(global_pairs_now.begin(), global_pairs_now.end()); } if(verbose) std::cout << "in collision " << local_result.numContacts() << ": " << std::endl; if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl; return true; } else { if(verbose) std::cout << "collision free " << std::endl; if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl; return false; } } template bool collide_Test_Oriented(const Transform3f& tf, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose) { BVHModel m1; BVHModel m2; m1.bv_splitter.reset(new BVSplitter(split_method)); m2.bv_splitter.reset(new BVSplitter(split_method)); m1.beginModel(); m1.addSubModel(vertices1, triangles1); m1.endModel(); m2.beginModel(); m2.addSubModel(vertices2, triangles2); m2.endModel(); Transform3f pose1(tf), pose2; CollisionResult local_result; TraversalNode node; if(!initialize(node, (const BVHModel&)m1, pose1, (const BVHModel&)m2, pose2, CollisionRequest(num_max_contacts, enable_contact), local_result)) std::cout << "initialize error" << std::endl; node.enable_statistics = verbose; collide(&node); if(local_result.numContacts() > 0) { if(global_pairs.size() == 0) { local_result.getContacts(global_pairs); std::sort(global_pairs.begin(), global_pairs.end()); } else { local_result.getContacts(global_pairs_now); std::sort(global_pairs_now.begin(), global_pairs_now.end()); } if(verbose) std::cout << "in collision " << local_result.numContacts() << ": " << std::endl; if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl; return true; } else { if(verbose) std::cout << "collision free " << std::endl; if(verbose) std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl; return false; } } template bool test_collide_func(const Transform3f& tf, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method) { BVHModel m1; BVHModel m2; m1.bv_splitter.reset(new BVSplitter(split_method)); m2.bv_splitter.reset(new BVSplitter(split_method)); m1.beginModel(); m1.addSubModel(vertices1, triangles1); m1.endModel(); m2.beginModel(); m2.addSubModel(vertices2, triangles2); m2.endModel(); Transform3f pose1(tf), pose2; std::vector contacts; CollisionRequest request(num_max_contacts, enable_contact); CollisionResult result; int num_contacts = collide(&m1, pose1, &m2, pose2, request, result); result.getContacts(contacts); global_pairs_now.resize(num_contacts); for(int i = 0; i < num_contacts; ++i) { global_pairs_now[i].b1 = contacts[i].b1; global_pairs_now[i].b2 = contacts[i].b2; } std::sort(global_pairs_now.begin(), global_pairs_now.end()); if(num_contacts > 0) return true; else return false; } fcl-0.5.0/test/test_fcl_distance.cpp000066400000000000000000000476051274356571000174460ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #define BOOST_TEST_MODULE "FCL_DISTANCE" #include #include "fcl/traversal/traversal_node_bvhs.h" #include "fcl/traversal/traversal_node_setup.h" #include "fcl/collision_node.h" #include "test_fcl_utility.h" #include #include "fcl_resources/config.h" #include using namespace fcl; bool verbose = false; FCL_REAL DELTA = 0.001; template void distance_Test(const Transform3f& tf, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, int qsize, DistanceRes& distance_result, bool verbose = true); bool collide_Test_OBB(const Transform3f& tf, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose); template void distance_Test_Oriented(const Transform3f& tf, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, int qsize, DistanceRes& distance_result, bool verbose = true); inline bool nearlyEqual(const Vec3f& a, const Vec3f& b) { if(fabs(a[0] - b[0]) > DELTA) return false; if(fabs(a[1] - b[1]) > DELTA) return false; if(fabs(a[2] - b[2]) > DELTA) return false; return true; } BOOST_AUTO_TEST_CASE(mesh_distance) { std::vector p1, p2; std::vector t1, t2; boost::filesystem::path path(TEST_RESOURCES_DIR); loadOBJFile((path / "env.obj").string().c_str(), p1, t1); loadOBJFile((path / "rob.obj").string().c_str(), p2, t2); std::vector transforms; // t0 FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; std::size_t n = 10; generateRandomTransforms(extents, transforms, n); double dis_time = 0; double col_time = 0; DistanceRes res, res_now; for(std::size_t i = 0; i < transforms.size(); ++i) { boost::timer timer_col; collide_Test_OBB(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); col_time += timer_col.elapsed(); boost::timer timer_dist; distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res, verbose); dis_time += timer_dist.elapsed(); distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test_Oriented(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose); BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA); BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2))); } std::cout << "distance timing: " << dis_time << " sec" << std::endl; std::cout << "collision timing: " << col_time << " sec" << std::endl; } template void distance_Test_Oriented(const Transform3f& tf, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, int qsize, DistanceRes& distance_result, bool verbose) { BVHModel m1; BVHModel m2; m1.bv_splitter.reset(new BVSplitter(split_method)); m2.bv_splitter.reset(new BVSplitter(split_method)); m1.beginModel(); m1.addSubModel(vertices1, triangles1); m1.endModel(); m2.beginModel(); m2.addSubModel(vertices2, triangles2); m2.endModel(); DistanceResult local_result; TraversalNode node; if(!initialize(node, (const BVHModel&)m1, tf, (const BVHModel&)m2, Transform3f(), DistanceRequest(true), local_result)) std::cout << "initialize error" << std::endl; node.enable_statistics = verbose; distance(&node, NULL, qsize); // points are in local coordinate, to global coordinate Vec3f p1 = local_result.nearest_points[0]; Vec3f p2 = local_result.nearest_points[1]; distance_result.distance = local_result.min_distance; distance_result.p1 = p1; distance_result.p2 = p2; if(verbose) { std::cout << "distance " << local_result.min_distance << std::endl; std::cout << p1[0] << " " << p1[1] << " " << p1[2] << std::endl; std::cout << p2[0] << " " << p2[1] << " " << p2[2] << std::endl; std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl; } } template void distance_Test(const Transform3f& tf, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, int qsize, DistanceRes& distance_result, bool verbose) { BVHModel m1; BVHModel m2; m1.bv_splitter.reset(new BVSplitter(split_method)); m2.bv_splitter.reset(new BVSplitter(split_method)); m1.beginModel(); m1.addSubModel(vertices1, triangles1); m1.endModel(); m2.beginModel(); m2.addSubModel(vertices2, triangles2); m2.endModel(); Transform3f pose1(tf), pose2; DistanceResult local_result; MeshDistanceTraversalNode node; if(!initialize(node, m1, pose1, m2, pose2, DistanceRequest(true), local_result)) std::cout << "initialize error" << std::endl; node.enable_statistics = verbose; distance(&node, NULL, qsize); distance_result.distance = local_result.min_distance; distance_result.p1 = local_result.nearest_points[0]; distance_result.p2 = local_result.nearest_points[1]; if(verbose) { std::cout << "distance " << local_result.min_distance << std::endl; std::cout << local_result.nearest_points[0][0] << " " << local_result.nearest_points[0][1] << " " << local_result.nearest_points[0][2] << std::endl; std::cout << local_result.nearest_points[1][0] << " " << local_result.nearest_points[1][1] << " " << local_result.nearest_points[1][2] << std::endl; std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl; } } bool collide_Test_OBB(const Transform3f& tf, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose) { BVHModel m1; BVHModel m2; m1.bv_splitter.reset(new BVSplitter(split_method)); m2.bv_splitter.reset(new BVSplitter(split_method)); m1.beginModel(); m1.addSubModel(vertices1, triangles1); m1.endModel(); m2.beginModel(); m2.addSubModel(vertices2, triangles2); m2.endModel(); CollisionResult local_result; MeshCollisionTraversalNodeOBB node; if(!initialize(node, (const BVHModel&)m1, tf, (const BVHModel&)m2, Transform3f(), CollisionRequest(), local_result)) std::cout << "initialize error" << std::endl; node.enable_statistics = verbose; collide(&node); if(local_result.numContacts() > 0) return true; else return false; } fcl-0.5.0/test/test_fcl_frontlist.cpp000066400000000000000000000356751274356571000177040ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #define BOOST_TEST_MODULE "FCL_FRONT_LIST" #include #include "fcl/traversal/traversal_node_bvhs.h" #include "fcl/traversal/traversal_node_setup.h" #include "fcl/collision_node.h" #include "test_fcl_utility.h" #include "fcl_resources/config.h" #include using namespace fcl; template bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool refit_bottomup, bool verbose); template bool collide_front_list_Test_Oriented(const Transform3f& tf1, const Transform3f& tf2, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose); template bool collide_Test(const Transform3f& tf, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose); // TODO: randomly still have some runtime error BOOST_AUTO_TEST_CASE(front_list) { std::vector p1, p2; std::vector t1, t2; boost::filesystem::path path(TEST_RESOURCES_DIR); loadOBJFile((path / "env.obj").string().c_str(), p1, t1); loadOBJFile((path / "rob.obj").string().c_str(), p2, t2); std::vector transforms; // t0 std::vector transforms2; // t1 FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; FCL_REAL delta_trans[] = {1, 1, 1}; std::size_t n = 10; bool verbose = false; generateRandomTransforms(extents, delta_trans, 0.005 * 2 * 3.1415, transforms, transforms2, n); bool res, res2; for(std::size_t i = 0; i < transforms.size(); ++i) { res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose); BOOST_CHECK(res == res2); res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose); BOOST_CHECK(res == res2); res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose); BOOST_CHECK(res == res2); } for(std::size_t i = 0; i < transforms.size(); ++i) { res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose); BOOST_CHECK(res == res2); res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose); BOOST_CHECK(res == res2); res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose); BOOST_CHECK(res == res2); } for(std::size_t i = 0; i < transforms.size(); ++i) { // Disabled broken test lines. Please see #25. // res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); // res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose); // BOOST_CHECK(res == res2); res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose); BOOST_CHECK(res == res2); res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); res2 = collide_front_list_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose); BOOST_CHECK(res == res2); } for(std::size_t i = 0; i < transforms.size(); ++i) { res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose); BOOST_CHECK(res == res2); res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose); BOOST_CHECK(res == res2); res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose); BOOST_CHECK(res == res2); } for(std::size_t i = 0; i < transforms.size(); ++i) { res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose); BOOST_CHECK(res == res2); res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose); BOOST_CHECK(res == res2); res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose); BOOST_CHECK(res == res2); } for(std::size_t i = 0; i < transforms.size(); ++i) { res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose); BOOST_CHECK(res == res2); res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose); BOOST_CHECK(res == res2); res = collide_Test >(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); res2 = collide_front_list_Test >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, false, verbose); BOOST_CHECK(res == res2); } for(std::size_t i = 0; i < transforms.size(); ++i) { res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); res2 = collide_front_list_Test_Oriented(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); BOOST_CHECK(res == res2); res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); res2 = collide_front_list_Test_Oriented(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); BOOST_CHECK(res == res2); res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); res2 = collide_front_list_Test_Oriented(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); BOOST_CHECK(res == res2); } for(std::size_t i = 0; i < transforms.size(); ++i) { res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); res2 = collide_front_list_Test_Oriented(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose); BOOST_CHECK(res == res2); res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); res2 = collide_front_list_Test_Oriented(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose); BOOST_CHECK(res == res2); res = collide_Test(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); res2 = collide_front_list_Test_Oriented(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, verbose); BOOST_CHECK(res == res2); } } template bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool refit_bottomup, bool verbose) { BVHModel m1; BVHModel m2; m1.bv_splitter.reset(new BVSplitter(split_method)); m2.bv_splitter.reset(new BVSplitter(split_method)); BVHFrontList front_list; std::vector vertices1_new(vertices1.size()); for(std::size_t i = 0; i < vertices1_new.size(); ++i) { vertices1_new[i] = tf1.transform(vertices1[i]); } m1.beginModel(); m1.addSubModel(vertices1_new, triangles1); m1.endModel(); m2.beginModel(); m2.addSubModel(vertices2, triangles2); m2.endModel(); Transform3f pose1, pose2; CollisionResult local_result; MeshCollisionTraversalNode node; if(!initialize(node, m1, pose1, m2, pose2, CollisionRequest(std::numeric_limits::max(), false), local_result)) std::cout << "initialize error" << std::endl; node.enable_statistics = verbose; collide(&node, &front_list); if(verbose) std::cout << "front list size " << front_list.size() << std::endl; // update the mesh for(std::size_t i = 0; i < vertices1.size(); ++i) { vertices1_new[i] = tf2.transform(vertices1[i]); } m1.beginReplaceModel(); m1.replaceSubModel(vertices1_new); m1.endReplaceModel(true, refit_bottomup); m2.beginReplaceModel(); m2.replaceSubModel(vertices2); m2.endReplaceModel(true, refit_bottomup); local_result.clear(); collide(&node, &front_list); if(local_result.numContacts() > 0) return true; else return false; } template bool collide_front_list_Test_Oriented(const Transform3f& tf1, const Transform3f& tf2, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose) { BVHModel m1; BVHModel m2; m1.bv_splitter.reset(new BVSplitter(split_method)); m2.bv_splitter.reset(new BVSplitter(split_method)); BVHFrontList front_list; m1.beginModel(); m1.addSubModel(vertices1, triangles1); m1.endModel(); m2.beginModel(); m2.addSubModel(vertices2, triangles2); m2.endModel(); Transform3f pose1(tf1), pose2; CollisionResult local_result; TraversalNode node; if(!initialize(node, (const BVHModel&)m1, pose1, (const BVHModel&)m2, pose2, CollisionRequest(std::numeric_limits::max(), false), local_result)) std::cout << "initialize error" << std::endl; node.enable_statistics = verbose; collide(&node, &front_list); if(verbose) std::cout << "front list size " << front_list.size() << std::endl; // update the mesh pose1 = tf2; if(!initialize(node, (const BVHModel&)m1, pose1, (const BVHModel&)m2, pose2, CollisionRequest(), local_result)) std::cout << "initialize error" << std::endl; local_result.clear(); collide(&node, &front_list); if(local_result.numContacts() > 0) return true; else return false; } template bool collide_Test(const Transform3f& tf, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2, SplitMethodType split_method, bool verbose) { BVHModel m1; BVHModel m2; m1.bv_splitter.reset(new BVSplitter(split_method)); m2.bv_splitter.reset(new BVSplitter(split_method)); m1.beginModel(); m1.addSubModel(vertices1, triangles1); m1.endModel(); m2.beginModel(); m2.addSubModel(vertices2, triangles2); m2.endModel(); Transform3f pose1(tf), pose2; CollisionResult local_result; MeshCollisionTraversalNode node; if(!initialize(node, m1, pose1, m2, pose2, CollisionRequest(std::numeric_limits::max(), false), local_result)) std::cout << "initialize error" << std::endl; node.enable_statistics = verbose; collide(&node); if(local_result.numContacts() > 0) return true; else return false; } fcl-0.5.0/test/test_fcl_geometric_shapes.cpp000077500000000000000000005171041274356571000211740ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #define BOOST_TEST_MODULE "FCL_GEOMETRIC_SHAPES" #include #include "fcl/narrowphase/narrowphase.h" #include "fcl/collision.h" #include "test_fcl_utility.h" #include "fcl/ccd/motion.h" #include #include using namespace fcl; FCL_REAL extents [6] = {0, 0, 0, 10, 10, 10}; GJKSolver_libccd solver1; GJKSolver_indep solver2; #define BOOST_CHECK_FALSE(p) BOOST_CHECK(!(p)) BOOST_AUTO_TEST_CASE(sphere_shape) { const double tol = 1e-12; const double radius = 5.0; const double pi = constants::pi; Sphere s(radius); const double volume = 4.0 / 3.0 * pi * radius * radius * radius; BOOST_CHECK_CLOSE(volume, s.computeVolume(), tol); } BOOST_AUTO_TEST_CASE(gjkcache) { Cylinder s1(5, 10); Cone s2(5, 10); CollisionRequest request; request.enable_cached_gjk_guess = true; request.gjk_solver_type = GST_INDEP; TranslationMotion motion(Transform3f(Vec3f(-20.0, -20.0, -20.0)), Transform3f(Vec3f(20.0, 20.0, 20.0))); int N = 1000; FCL_REAL dt = 1.0 / (N - 1); /// test exploiting spatial coherence Timer timer1; timer1.start(); std::vector result1(N); for(int i = 0; i < N; ++i) { motion.integrate(dt * i); Transform3f tf; motion.getCurrentTransform(tf); CollisionResult result; collide(&s1, Transform3f(), &s2, tf, request, result); result1[i] = result.isCollision(); request.cached_gjk_guess = result.cached_gjk_guess; // use cached guess } timer1.stop(); /// test without exploiting spatial coherence Timer timer2; timer2.start(); std::vector result2(N); request.enable_cached_gjk_guess = false; for(int i = 0; i < N; ++i) { motion.integrate(dt * i); Transform3f tf; motion.getCurrentTransform(tf); CollisionResult result; collide(&s1, Transform3f(), &s2, tf, request, result); result2[i] = result.isCollision(); } timer2.stop(); std::cout << timer1.getElapsedTime() << " " << timer2.getElapsedTime() << std::endl; for(std::size_t i = 0; i < result1.size(); ++i) { BOOST_CHECK(result1[i] == result2[i]); } } template void printComparisonError(const std::string& comparison_type, const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2, GJKSolverType solver_type, const Vec3f& expected_contact_or_normal, const Vec3f& actual_contact_or_normal, bool check_opposite_normal, FCL_REAL tol) { std::cout << "Disagreement between " << comparison_type << " and expected_" << comparison_type << " for " << getNodeTypeName(s1.getNodeType()) << " and " << getNodeTypeName(s2.getNodeType()) << " with '" << getGJKSolverName(solver_type) << "' solver." << std::endl << "tf1.quaternion: " << tf1.getQuatRotation() << std::endl << "tf1.translation: " << tf1.getTranslation() << std::endl << "tf2.quaternion: " << tf2.getQuatRotation() << std::endl << "tf2.translation: " << tf2.getTranslation() << std::endl << "expected_" << comparison_type << ": " << expected_contact_or_normal << "actual_" << comparison_type << " : " << actual_contact_or_normal << std::endl; if (check_opposite_normal) std::cout << " or " << -expected_contact_or_normal; std::cout << std::endl << "difference: " << (actual_contact_or_normal - expected_contact_or_normal).norm() << std::endl << "tolerance: " << tol << std::endl; } template void printComparisonError(const std::string& comparison_type, const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2, GJKSolverType solver_type, FCL_REAL expected_depth, FCL_REAL actual_depth, FCL_REAL tol) { std::cout << "Disagreement between " << comparison_type << " and expected_" << comparison_type << " for " << getNodeTypeName(s1.getNodeType()) << " and " << getNodeTypeName(s2.getNodeType()) << " with '" << getGJKSolverName(solver_type) << "' solver." << std::endl << "tf1.quaternion: " << tf1.getQuatRotation() << std::endl << "tf1.translation: " << tf1.getTranslation() << std::endl << "tf2.quaternion: " << tf2.getQuatRotation() << std::endl << "tf2.translation: " << tf2.getTranslation() << std::endl << "expected_depth: " << expected_depth << std::endl << "actual_depth : " << actual_depth << std::endl << "difference: " << std::abs(actual_depth - expected_depth) << std::endl << "tolerance: " << tol << std::endl; } template bool checkContactPoints(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2, GJKSolverType solver_type, const ContactPoint& expected, const ContactPoint& actual, bool check_position = false, bool check_depth = false, bool check_normal = false, bool check_opposite_normal = false, FCL_REAL tol = 1e-9) { if (check_position) { bool contact_equal = actual.pos.equal(expected.pos, tol); if (!contact_equal) return false; } if (check_depth) { bool depth_equal = std::abs(actual.penetration_depth - expected.penetration_depth) < tol; if (!depth_equal) return false; } if (check_normal) { bool normal_equal = actual.normal.equal(expected.normal, tol); if (!normal_equal && check_opposite_normal) normal_equal = actual.normal.equal(-expected.normal, tol); if (!normal_equal) return false; } return true; } template bool inspectContactPoints(const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2, GJKSolverType solver_type, const std::vector& expected_contacts, const std::vector& actual_contacts, bool check_position = false, bool check_depth = false, bool check_normal = false, bool check_opposite_normal = false, FCL_REAL tol = 1e-9) { // Check number of contact points bool sameNumContacts = (actual_contacts.size() == expected_contacts.size()); BOOST_CHECK(sameNumContacts); if (!sameNumContacts) { std::cout << "\n" << "===== [ geometric shape collision test failure report ] ======\n" << "\n" << "Solver type: " << getGJKSolverName(solver_type) << "\n" << "\n" << "[ Shape 1 ]\n" << "Shape type : " << getNodeTypeName(s1.getNodeType()) << "\n" << "tf1.quaternion : " << tf1.getQuatRotation() << "\n" << "tf1.translation: " << tf1.getTranslation() << "\n" << "\n" << "[ Shape 2 ]\n" << "Shape type : " << getNodeTypeName(s2.getNodeType()) << "\n" << "tf2.quaternion : " << tf2.getQuatRotation() << "\n" << "tf2.translation: " << tf2.getTranslation() << "\n" << "\n" << "The numbers of expected contacts '" << expected_contacts.size() << "' and the number of actual contacts '" << actual_contacts.size() << "' are not equal.\n" << "\n"; return false; } // Check if actual contacts and expected contacts are matched const size_t numContacts = actual_contacts.size(); std::vector index_to_actual_contacts(numContacts, -1); std::vector index_to_expected_contacts(numContacts, -1); bool foundAll = true; for (size_t i = 0; i < numContacts; ++i) { const ContactPoint& expected = expected_contacts[i]; // Check if expected contact is in the list of actual contacts for (size_t j = 0; j < numContacts; ++j) { if (index_to_expected_contacts[j] != -1) continue; const ContactPoint& actual = actual_contacts[j]; bool found = checkContactPoints( s1, tf1, s2, tf2, solver_type, expected, actual, check_position, check_depth, check_normal, check_opposite_normal, tol); if (found) { index_to_actual_contacts[i] = j; index_to_expected_contacts[j] = i; break; } } if (index_to_actual_contacts[i] == -1) foundAll = false; } if (!foundAll) { std::cout << "\n" << "===== [ geometric shape collision test failure report ] ======\n" << "\n" << "Solver type: " << getGJKSolverName(solver_type) << "\n" << "\n" << "[ Shape 1 ]\n" << "Shape type : " << getNodeTypeName(s1.getNodeType()) << "\n" << "tf1.quaternion : " << tf1.getQuatRotation() << "\n" << "tf1.translation: " << tf1.getTranslation() << "\n" << "\n" << "[ Shape 2 ]\n" << "Shape type : " << getNodeTypeName(s2.getNodeType()) << "\n" << "tf2.quaternion : " << tf2.getQuatRotation() << "\n" << "tf2.translation: " << tf2.getTranslation() << "\n" << "\n" << "[ Expected Contacts: " << numContacts << " ]\n"; for (size_t i = 0; i < numContacts; ++i) { const ContactPoint& expected = expected_contacts[i]; std::cout << "(" << i << ") pos: " << expected.pos << ", " << "normal: " << expected.normal << ", " << "depth: " << expected.penetration_depth << " ---- "; if (index_to_actual_contacts[i] != -1) std::cout << "found, actual (" << index_to_actual_contacts[i] << ")\n"; else std::cout << "not found!\n"; } std::cout << "\n" << "[ Actual Contacts: " << numContacts << " ]\n"; for (size_t i = 0; i < numContacts; ++i) { const ContactPoint& actual = actual_contacts[i]; std::cout << "(" << i << ") pos: " << actual.pos << ", " << "normal: " << actual.normal << ", " << "depth: " << actual.penetration_depth << " ---- "; if (index_to_expected_contacts[i] != -1) std::cout << "found, expected (" << index_to_expected_contacts[i] << ")\n"; else std::cout << "not found!\n"; } std::cout << "\n"; } return foundAll; } void getContactPointsFromResult(std::vector& contacts, const CollisionResult& result) { const size_t numContacts = result.numContacts(); contacts.resize(numContacts); for (size_t i = 0; i < numContacts; ++i) { const Contact& cnt = result.getContact(i); contacts[i].pos = cnt.pos; contacts[i].normal = cnt.normal; contacts[i].penetration_depth = cnt.penetration_depth; } } template void testShapeIntersection( const S1& s1, const Transform3f& tf1, const S2& s2, const Transform3f& tf2, GJKSolverType solver_type, bool expected_res, const std::vector& expected_contacts = std::vector(), bool check_position = true, bool check_depth = true, bool check_normal = true, bool check_opposite_normal = false, FCL_REAL tol = 1e-9) { CollisionRequest request; request.gjk_solver_type = solver_type; request.num_max_contacts = std::numeric_limits::max(); CollisionResult result; std::vector actual_contacts; bool res; // Part A: Check collisions using shapeIntersect() // Check only whether they are colliding or not. if (solver_type == GST_LIBCCD) { res = solver1.shapeIntersect(s1, tf1, s2, tf2, NULL); } else if (solver_type == GST_INDEP) { res = solver2.shapeIntersect(s1, tf1, s2, tf2, NULL); } else { std::cerr << "Invalid GJK solver. Test aborted." << std::endl; return; } BOOST_CHECK_EQUAL(res, expected_res); // Check contact information as well if (solver_type == GST_LIBCCD) { res = solver1.shapeIntersect(s1, tf1, s2, tf2, &actual_contacts); } else if (solver_type == GST_INDEP) { res = solver2.shapeIntersect(s1, tf1, s2, tf2, &actual_contacts); } else { std::cerr << "Invalid GJK solver. Test aborted." << std::endl; return; } BOOST_CHECK_EQUAL(res, expected_res); if (expected_res) { BOOST_CHECK(inspectContactPoints(s1, tf1, s2, tf2, solver_type, expected_contacts, actual_contacts, check_position, check_depth, check_normal, check_opposite_normal, tol)); } // Part B: Check collisions using collide() // Check only whether they are colliding or not. request.enable_contact = false; result.clear(); res = (collide(&s1, tf1, &s2, tf2, request, result) > 0); BOOST_CHECK_EQUAL(res, expected_res); // Check contact information as well request.enable_contact = true; result.clear(); res = (collide(&s1, tf1, &s2, tf2, request, result) > 0); BOOST_CHECK_EQUAL(res, expected_res); if (expected_res) { getContactPointsFromResult(actual_contacts, result); BOOST_CHECK(inspectContactPoints(s1, tf1, s2, tf2, solver_type, expected_contacts, actual_contacts, check_position, check_depth, check_normal, check_opposite_normal, tol)); } } // Shape intersection test coverage (libccd) // // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | box | O | O | | | | | O | O | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | sphere |/////| O | | | | | O | O | O | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | ellipsoid |/////|////////| O | | | | O | O | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | capsule |/////|////////|///////////| | | | O | O | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | cone |/////|////////|///////////|/////////| O | O | O | O | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | cylinder |/////|////////|///////////|/////////|//////| O | O | O | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | plane |/////|////////|///////////|/////////|//////|//////////| | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | half-space |/////|////////|///////////|/////////|//////|//////////|///////| | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ BOOST_AUTO_TEST_CASE(shapeIntersection_spheresphere) { Sphere s1(20); Sphere s2(10); Transform3f tf1; Transform3f tf2; Transform3f transform; generateRandomTransform(extents, transform); std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(Vec3f(40, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(40, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(30, 0, 0)); contacts.resize(1); contacts[0].normal.setValue(1, 0, 0); contacts[0].pos.setValue(20, 0, 0); contacts[0].penetration_depth = 0.0; testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(30.01, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(30.01, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(29.9, 0, 0)); contacts.resize(1); contacts[0].normal.setValue(1, 0, 0); contacts[0].pos.setValue(20.0 - 0.1 * 20.0/(20.0 + 10.0), 0, 0); contacts[0].penetration_depth = 0.1; testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(29.9, 0, 0)); contacts.resize(1); contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); contacts[0].pos = transform.transform(Vec3f(20.0 - 0.1 * 20.0/(20.0 + 10.0), 0, 0)); contacts[0].penetration_depth = 0.1; testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(); contacts.resize(1); contacts[0].normal.setZero(); // If the centers of two sphere are at the same position, the normal is (0, 0, 0) contacts[0].pos.setZero(); contacts[0].penetration_depth = 20.0 + 10.0; testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform; contacts.resize(1); contacts[0].normal.setZero(); // If the centers of two sphere are at the same position, the normal is (0, 0, 0) contacts[0].pos = transform.transform(Vec3f()); contacts[0].penetration_depth = 20.0 + 10.0; testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-29.9, 0, 0)); contacts.resize(1); contacts[0].normal.setValue(-1, 0, 0); contacts[0].pos.setValue(-20.0 + 0.1 * 20.0/(20.0 + 10.0), 0, 0); contacts[0].penetration_depth = 0.1; testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-29.9, 0, 0)); contacts.resize(1); contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); contacts[0].pos = transform.transform(Vec3f(-20.0 + 0.1 * 20.0/(20.0 + 10.0), 0, 0)); contacts[0].penetration_depth = 0.1; testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-30.0, 0, 0)); contacts.resize(1); contacts[0].normal.setValue(-1, 0, 0); contacts[0].pos.setValue(-20, 0, 0); contacts[0].penetration_depth = 0.0; testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-30.01, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-30.01, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); } bool compareContactPoints1(const Vec3f& c1,const Vec3f& c2) { return c1[2] < c2[2]; } // Ascending order bool compareContactPoints2(const ContactPoint& cp1,const ContactPoint& cp2) { return cp1.pos[2] < cp2.pos[2]; } // Ascending order void testBoxBoxContactPoints(const Matrix3f& R) { Box s1(100, 100, 100); Box s2(10, 20, 30); // Vertices of s2 std::vector vertices(8); vertices[0].setValue( 1, 1, 1); vertices[1].setValue( 1, 1, -1); vertices[2].setValue( 1, -1, 1); vertices[3].setValue( 1, -1, -1); vertices[4].setValue(-1, 1, 1); vertices[5].setValue(-1, 1, -1); vertices[6].setValue(-1, -1, 1); vertices[7].setValue(-1, -1, -1); for (int i = 0; i < 8; ++i) { vertices[i][0] *= 0.5 * s2.side[0]; vertices[i][1] *= 0.5 * s2.side[1]; vertices[i][2] *= 0.5 * s2.side[2]; } Transform3f tf1 = Transform3f(Vec3f(0, 0, -50)); Transform3f tf2 = Transform3f(R); std::vector contacts; // Make sure the two boxes are colliding bool res = solver1.shapeIntersect(s1, tf1, s2, tf2, &contacts); BOOST_CHECK(res); // Compute global vertices for (int i = 0; i < 8; ++i) vertices[i] = tf2.transform(vertices[i]); // Sort the vertices so that the lowest vertex along z-axis comes first std::sort(vertices.begin(), vertices.end(), compareContactPoints1); std::sort(contacts.begin(), contacts.end(), compareContactPoints2); // The lowest n vertex along z-axis should be the contact point size_t numContacts = contacts.size(); numContacts = std::min(static_cast(1), numContacts); // TODO: BoxBox algorithm seems not able to find all the colliding vertices. // We just check the deepest one as workaround. for (size_t i = 0; i < numContacts; ++i) { BOOST_CHECK(vertices[i].equal(contacts[i].pos)); BOOST_CHECK(Vec3f(0, 0, 1).equal(contacts[i].normal)); } } BOOST_AUTO_TEST_CASE(shapeIntersection_boxbox) { Box s1(20, 40, 50); Box s2(10, 10, 10); Transform3f tf1; Transform3f tf2; Transform3f transform; generateRandomTransform(extents, transform); std::vector contacts; Quaternion3f q; q.fromAxisAngle(Vec3f(0, 0, 1), (FCL_REAL)3.140 / 6); tf1 = Transform3f(); tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0). contacts.resize(4); contacts[0].normal.setValue(1, 0, 0); contacts[1].normal.setValue(1, 0, 0); contacts[2].normal.setValue(1, 0, 0); contacts[3].normal.setValue(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0). contacts.resize(4); contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); contacts[1].normal = transform.getRotation() * Vec3f(1, 0, 0); contacts[2].normal = transform.getRotation() * Vec3f(1, 0, 0); contacts[3].normal = transform.getRotation() * Vec3f(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(15, 0, 0)); contacts.resize(4); contacts[0].normal = Vec3f(1, 0, 0); contacts[1].normal = Vec3f(1, 0, 0); contacts[2].normal = Vec3f(1, 0, 0); contacts[3].normal = Vec3f(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(15.01, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(q); contacts.resize(4); contacts[0].normal = Vec3f(1, 0, 0); contacts[1].normal = Vec3f(1, 0, 0); contacts[2].normal = Vec3f(1, 0, 0); contacts[3].normal = Vec3f(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = transform; tf2 = transform * Transform3f(q); contacts.resize(4); contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); contacts[1].normal = transform.getRotation() * Vec3f(1, 0, 0); contacts[2].normal = transform.getRotation() * Vec3f(1, 0, 0); contacts[3].normal = transform.getRotation() * Vec3f(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); FCL_UINT32 numTests = 1e+2; for (FCL_UINT32 i = 0; i < numTests; ++i) { Transform3f tf; generateRandomTransform(extents, tf); testBoxBoxContactPoints(tf.getRotation()); } } BOOST_AUTO_TEST_CASE(shapeIntersection_spherebox) { Sphere s1(20); Box s2(5, 5, 5); Transform3f tf1; Transform3f tf2; Transform3f transform; generateRandomTransform(extents, transform); std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (-1, 0, 0). contacts.resize(1); contacts[0].normal.setValue(-1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(22.5, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(22.501, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(22.4, 0, 0)); contacts.resize(1); contacts[0].normal.setValue(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = transform; tf2 = transform * Transform3f(Vec3f(22.4, 0, 0)); contacts.resize(1); contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 1e-4); } BOOST_AUTO_TEST_CASE(shapeIntersection_spherecapsule) { Sphere s1(20); Capsule s2(5, 10); Transform3f tf1; Transform3f tf2; Transform3f transform; generateRandomTransform(extents, transform); std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(24.9, 0, 0)); contacts.resize(1); contacts[0].normal.setValue(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = transform; tf2 = transform * Transform3f(Vec3f(24.9, 0, 0)); contacts.resize(1); contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(25, 0, 0)); contacts.resize(1); contacts[0].normal.setValue(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = transform; tf2 = transform * Transform3f(Vec3f(25, 0, 0)); contacts.resize(1); contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(25.1, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(25.1, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); } BOOST_AUTO_TEST_CASE(shapeIntersection_cylindercylinder) { Cylinder s1(5, 10); Cylinder s2(5, 10); Transform3f tf1; Transform3f tf2; Transform3f transform; generateRandomTransform(extents, transform); std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(9.9, 0, 0)); contacts.resize(1); contacts[0].normal.setValue(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = transform; tf2 = transform * Transform3f(Vec3f(9.9, 0, 0)); contacts.resize(1); contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 1e-5); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(10.01, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(10.01, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); } BOOST_AUTO_TEST_CASE(shapeIntersection_conecone) { Cone s1(5, 10); Cone s2(5, 10); Transform3f tf1; Transform3f tf2; Transform3f transform; generateRandomTransform(extents, transform); std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(9.9, 0, 0)); contacts.resize(1); contacts[0].normal.setValue(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = transform; tf2 = transform * Transform3f(Vec3f(9.9, 0, 0)); contacts.resize(1); contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 1e-5); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(10.001, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(10.001, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 9.9)); contacts.resize(1); contacts[0].normal.setValue(0, 0, 1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 9.9)); contacts.resize(1); contacts[0].normal = transform.getRotation() * Vec3f(0, 0, 1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 1e-5); } BOOST_AUTO_TEST_CASE(shapeIntersection_cylindercone) { Cylinder s1(5, 10); Cone s2(5, 10); Transform3f tf1; Transform3f tf2; Transform3f transform; generateRandomTransform(extents, transform); std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(9.9, 0, 0)); contacts.resize(1); contacts[0].normal.setValue(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 0.061); tf1 = transform; tf2 = transform * Transform3f(Vec3f(9.9, 0, 0)); contacts.resize(1); contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 0.46); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(10.01, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(10.01, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 9.9)); contacts.resize(1); contacts[0].normal.setValue(0, 0, 1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 9.9)); contacts.resize(1); contacts[0].normal = transform.getRotation() * Vec3f(0, 0, 1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 1e-5); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 10.01)); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 10.01)); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); } BOOST_AUTO_TEST_CASE(shapeIntersection_ellipsoidellipsoid) { Ellipsoid s1(20, 40, 50); Ellipsoid s2(10, 10, 10); Transform3f tf1; Transform3f tf2; Transform3f transform; generateRandomTransform(extents, transform); Transform3f identity; std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(Vec3f(40, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(40, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(30, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(30.01, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(29.99, 0, 0)); contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(29.9, 0, 0)); contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); tf1 = Transform3f(); tf2 = Transform3f(); contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); tf1 = transform; tf2 = transform; contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-29.99, 0, 0)); contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-29.99, 0, 0)); contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-30, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-30.01, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false); } BOOST_AUTO_TEST_CASE(shapeIntersection_spheretriangle) { Sphere s(10); Vec3f t[3]; t[0].setValue(20, 0, 0); t[1].setValue(-20, 0, 0); t[2].setValue(0, 20, 0); Transform3f transform; generateRandomTransform(extents, transform); Vec3f normal; bool res; res = solver1.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, NULL); BOOST_CHECK(res); res = solver1.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL); BOOST_CHECK(res); t[0].setValue(30, 0, 0); t[1].setValue(9.9, -20, 0); t[2].setValue(9.9, 20, 0); res = solver1.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, NULL); BOOST_CHECK(res); res = solver1.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL); BOOST_CHECK(res); res = solver1.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, &normal); BOOST_CHECK(res); BOOST_CHECK(normal.equal(Vec3f(1, 0, 0), 1e-9)); res = solver1.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, &normal); BOOST_CHECK(res); BOOST_CHECK(normal.equal(transform.getRotation() * Vec3f(1, 0, 0), 1e-9)); } BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacetriangle) { Halfspace hs(Vec3f(1, 0, 0), 0); Vec3f t[3]; t[0].setValue(20, 0, 0); t[1].setValue(-20, 0, 0); t[2].setValue(0, 20, 0); Transform3f transform; generateRandomTransform(extents, transform); // Vec3f point; // FCL_REAL depth; Vec3f normal; bool res; res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, NULL); BOOST_CHECK(res); res = solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL); BOOST_CHECK(res); t[0].setValue(20, 0, 0); t[1].setValue(0, -20, 0); t[2].setValue(0, 20, 0); res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, NULL); BOOST_CHECK(res); res = solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL); BOOST_CHECK(res); res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, &normal); BOOST_CHECK(res); BOOST_CHECK(normal.equal(Vec3f(1, 0, 0), 1e-9)); res = solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, &normal); BOOST_CHECK(res); BOOST_CHECK(normal.equal(transform.getRotation() * Vec3f(1, 0, 0), 1e-9)); } BOOST_AUTO_TEST_CASE(shapeIntersection_planetriangle) { Plane hs(Vec3f(1, 0, 0), 0); Vec3f t[3]; t[0].setValue(20, 0, 0); t[1].setValue(-20, 0, 0); t[2].setValue(0, 20, 0); Transform3f transform; generateRandomTransform(extents, transform); // Vec3f point; // FCL_REAL depth; Vec3f normal; bool res; res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, NULL); BOOST_CHECK(res); res = solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL); BOOST_CHECK(res); t[0].setValue(20, 0, 0); t[1].setValue(-0.1, -20, 0); t[2].setValue(-0.1, 20, 0); res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, NULL); BOOST_CHECK(res); res = solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL); BOOST_CHECK(res); res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, &normal); BOOST_CHECK(res); BOOST_CHECK(normal.equal(Vec3f(1, 0, 0), 1e-9)); res = solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, &normal); BOOST_CHECK(res); BOOST_CHECK(normal.equal(transform.getRotation() * Vec3f(1, 0, 0), 1e-9)); } BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacesphere) { Sphere s(10); Halfspace hs(Vec3f(1, 0, 0), 0); Transform3f tf1; Transform3f tf2; Transform3f transform; generateRandomTransform(extents, transform); std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); contacts.resize(1); contacts[0].pos.setValue(-5, 0, 0); contacts[0].penetration_depth = 10; contacts[0].normal.setValue(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform; contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(-5, 0, 0)); contacts[0].penetration_depth = 10; contacts[0].normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(5, 0, 0)); contacts.resize(1); contacts[0].pos.setValue(-2.5, 0, 0); contacts[0].penetration_depth = 15; contacts[0].normal.setValue(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(5, 0, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(-2.5, 0, 0)); contacts[0].penetration_depth = 15; contacts[0].normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-5, 0, 0)); contacts.resize(1); contacts[0].pos.setValue(-7.5, 0, 0); contacts[0].penetration_depth = 5; contacts[0].normal.setValue(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-5, 0, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(-7.5, 0, 0)); contacts[0].penetration_depth = 5; contacts[0].normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-10.1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-10.1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(10.1, 0, 0)); contacts.resize(1); contacts[0].pos.setValue(0.05, 0, 0); contacts[0].penetration_depth = 20.1; contacts[0].normal.setValue(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(10.1, 0, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0.05, 0, 0)); contacts[0].penetration_depth = 20.1; contacts[0].normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); } BOOST_AUTO_TEST_CASE(shapeIntersection_planesphere) { Sphere s(10); Plane hs(Vec3f(1, 0, 0), 0); Transform3f tf1; Transform3f tf2; Transform3f transform; generateRandomTransform(extents, transform); std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); contacts.resize(1); contacts[0].pos.setZero(); contacts[0].penetration_depth = 10; contacts[0].normal.setValue(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = transform; tf2 = transform; contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, 0)); contacts[0].penetration_depth = 10; contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(5, 0, 0)); contacts.resize(1); contacts[0].pos.setValue(5, 0, 0); contacts[0].penetration_depth = 5; contacts[0].normal.setValue(1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(5, 0, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(5, 0, 0)); contacts[0].penetration_depth = 5; contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-5, 0, 0)); contacts.resize(1); contacts[0].pos.setValue(-5, 0, 0); contacts[0].penetration_depth = 5; contacts[0].normal.setValue(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-5, 0, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(-5, 0, 0)); contacts[0].penetration_depth = 5; contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-10.1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-10.1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(10.1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(10.1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); } BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacebox) { Box s(5, 10, 20); Halfspace hs(Vec3f(1, 0, 0), 0); Transform3f tf1; Transform3f tf2; Transform3f transform; generateRandomTransform(extents, transform); std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); contacts.resize(1); contacts[0].pos.setValue(-1.25, 0, 0); contacts[0].penetration_depth = 2.5; contacts[0].normal.setValue(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform; contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(-1.25, 0, 0)); contacts[0].penetration_depth = 2.5; contacts[0].normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(1.25, 0, 0)); contacts.resize(1); contacts[0].pos.setValue(-0.625, 0, 0); contacts[0].penetration_depth = 3.75; contacts[0].normal.setValue(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(1.25, 0, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(-0.625, 0, 0)); contacts[0].penetration_depth = 3.75; contacts[0].normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-1.25, 0, 0)); contacts.resize(1); contacts[0].pos.setValue(-1.875, 0, 0); contacts[0].penetration_depth = 1.25; contacts[0].normal.setValue(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-1.25, 0, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(-1.875, 0, 0)); contacts[0].penetration_depth = 1.25; contacts[0].normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(2.51, 0, 0)); contacts.resize(1); contacts[0].pos.setValue(0.005, 0, 0); contacts[0].penetration_depth = 5.01; contacts[0].normal.setValue(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(2.51, 0, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0.005, 0, 0)); contacts[0].penetration_depth = 5.01; contacts[0].normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-2.51, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-2.51, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = Transform3f(transform.getQuatRotation()); tf2 = Transform3f(); contacts.resize(1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, false, false, false); } BOOST_AUTO_TEST_CASE(shapeIntersection_planebox) { Box s(5, 10, 20); Plane hs(Vec3f(1, 0, 0), 0); Transform3f tf1; Transform3f tf2; Transform3f transform; generateRandomTransform(extents, transform); std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); contacts.resize(1); contacts[0].pos.setValue(0, 0, 0); contacts[0].penetration_depth = 2.5; contacts[0].normal.setValue(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = transform; tf2 = transform; contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, 0)); contacts[0].penetration_depth = 2.5; contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(1.25, 0, 0)); contacts.resize(1); contacts[0].pos.setValue(1.25, 0, 0); contacts[0].penetration_depth = 1.25; contacts[0].normal.setValue(1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(1.25, 0, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(1.25, 0, 0)); contacts[0].penetration_depth = 1.25; contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-1.25, 0, 0)); contacts.resize(1); contacts[0].pos.setValue(-1.25, 0, 0); contacts[0].penetration_depth = 1.25; contacts[0].normal.setValue(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-1.25, 0, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(-1.25, 0, 0)); contacts[0].penetration_depth = 1.25; contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(2.51, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(2.51, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-2.51, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-2.51, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = Transform3f(transform.getQuatRotation()); tf2 = Transform3f(); contacts.resize(1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, false, false, false); } BOOST_AUTO_TEST_CASE(shapeIntersection_halfspaceellipsoid) { Ellipsoid s(5, 10, 20); Halfspace hs(Vec3f(1, 0, 0), 0); Transform3f tf1; Transform3f tf2; Transform3f transform; generateRandomTransform(extents, transform); std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); contacts.resize(1); contacts[0].pos.setValue(-2.5, 0, 0); contacts[0].penetration_depth = 5.0; contacts[0].normal.setValue(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform; contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(-2.5, 0, 0)); contacts[0].penetration_depth = 5.0; contacts[0].normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(1.25, 0, 0)); contacts.resize(1); contacts[0].pos.setValue(-1.875, 0, 0); contacts[0].penetration_depth = 6.25; contacts[0].normal.setValue(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(1.25, 0, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(-1.875, 0, 0)); contacts[0].penetration_depth = 6.25; contacts[0].normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-1.25, 0, 0)); contacts.resize(1); contacts[0].pos.setValue(-3.125, 0, 0); contacts[0].penetration_depth = 3.75; contacts[0].normal.setValue(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-1.25, 0, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(-3.125, 0, 0)); contacts[0].penetration_depth = 3.75; contacts[0].normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(5.01, 0, 0)); contacts.resize(1); contacts[0].pos.setValue(0.005, 0, 0); contacts[0].penetration_depth = 10.01; contacts[0].normal.setValue(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(5.01, 0, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0.005, 0, 0)); contacts[0].penetration_depth = 10.01; contacts[0].normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-5.01, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-5.01, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); hs = Halfspace(Vec3f(0, 1, 0), 0); tf1 = Transform3f(); tf2 = Transform3f(); contacts.resize(1); contacts[0].pos.setValue(0, -5.0, 0); contacts[0].penetration_depth = 10.0; contacts[0].normal.setValue(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform; contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, -5.0, 0)); contacts[0].penetration_depth = 10.0; contacts[0].normal = transform.getQuatRotation().transform(Vec3f(0, -1, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 1.25, 0)); contacts.resize(1); contacts[0].pos.setValue(0, -4.375, 0); contacts[0].penetration_depth = 11.25; contacts[0].normal.setValue(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 1.25, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, -4.375, 0)); contacts[0].penetration_depth = 11.25; contacts[0].normal = transform.getQuatRotation().transform(Vec3f(0, -1, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -1.25, 0)); contacts.resize(1); contacts[0].pos.setValue(0, -5.625, 0); contacts[0].penetration_depth = 8.75; contacts[0].normal.setValue(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -1.25, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, -5.625, 0)); contacts[0].penetration_depth = 8.75; contacts[0].normal = transform.getQuatRotation().transform(Vec3f(0, -1, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 10.01, 0)); contacts.resize(1); contacts[0].pos.setValue(0, 0.005, 0); contacts[0].penetration_depth = 20.01; contacts[0].normal.setValue(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 10.01, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0.005, 0)); contacts[0].penetration_depth = 20.01; contacts[0].normal = transform.getQuatRotation().transform(Vec3f(0, -1, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -10.01, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -10.01, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); hs = Halfspace(Vec3f(0, 0, 1), 0); tf1 = Transform3f(); tf2 = Transform3f(); contacts.resize(1); contacts[0].pos.setValue(0, 0, -10.0); contacts[0].penetration_depth = 20.0; contacts[0].normal.setValue(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform; contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, -10.0)); contacts[0].penetration_depth = 20.0; contacts[0].normal = transform.getQuatRotation().transform(Vec3f(0, 0, -1)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 1.25)); contacts.resize(1); contacts[0].pos.setValue(0, 0, -9.375); contacts[0].penetration_depth = 21.25; contacts[0].normal.setValue(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 1.25)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, -9.375)); contacts[0].penetration_depth = 21.25; contacts[0].normal = transform.getQuatRotation().transform(Vec3f(0, 0, -1)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -1.25)); contacts.resize(1); contacts[0].pos.setValue(0, 0, -10.625); contacts[0].penetration_depth = 18.75; contacts[0].normal.setValue(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -1.25)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, -10.625)); contacts[0].penetration_depth = 18.75; contacts[0].normal = transform.getQuatRotation().transform(Vec3f(0, 0, -1)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 20.01)); contacts.resize(1); contacts[0].pos.setValue(0, 0, 0.005); contacts[0].penetration_depth = 40.01; contacts[0].normal.setValue(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 20.01)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, 0.005)); contacts[0].penetration_depth = 40.01; contacts[0].normal = transform.getQuatRotation().transform(Vec3f(0, 0, -1)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -20.01)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -20.01)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); } BOOST_AUTO_TEST_CASE(shapeIntersection_planeellipsoid) { Ellipsoid s(5, 10, 20); Plane hs(Vec3f(1, 0, 0), 0); Transform3f tf1; Transform3f tf2; Transform3f transform; generateRandomTransform(extents, transform); std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); contacts.resize(1); contacts[0].pos.setValue(0, 0, 0); contacts[0].penetration_depth = 5.0; contacts[0].normal.setValue(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = transform; tf2 = transform; contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, 0)); contacts[0].penetration_depth = 5.0; contacts[0].normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(1.25, 0, 0)); contacts.resize(1); contacts[0].pos.setValue(1.25, 0, 0); contacts[0].penetration_depth = 3.75; contacts[0].normal.setValue(1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(1.25, 0, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(1.25, 0, 0)); contacts[0].penetration_depth = 3.75; contacts[0].normal = transform.getQuatRotation().transform(Vec3f(1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-1.25, 0, 0)); contacts.resize(1); contacts[0].pos.setValue(-1.25, 0, 0); contacts[0].penetration_depth = 3.75; contacts[0].normal.setValue(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-1.25, 0, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(-1.25, 0, 0)); contacts[0].penetration_depth = 3.75; contacts[0].normal = transform.getQuatRotation().transform(Vec3f(-1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(5.01, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(5.01, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-5.01, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-5.01, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); hs = Plane(Vec3f(0, 1, 0), 0); tf1 = Transform3f(); tf2 = Transform3f(); contacts.resize(1); contacts[0].pos.setValue(0, 0.0, 0); contacts[0].penetration_depth = 10.0; contacts[0].normal.setValue(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = transform; tf2 = transform; contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, 0)); contacts[0].penetration_depth = 10.0; contacts[0].normal = transform.getQuatRotation().transform(Vec3f(0, -1, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 1.25, 0)); contacts.resize(1); contacts[0].pos.setValue(0, 1.25, 0); contacts[0].penetration_depth = 8.75; contacts[0].normal.setValue(0, 1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 1.25, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 1.25, 0)); contacts[0].penetration_depth = 8.75; contacts[0].normal = transform.getQuatRotation().transform(Vec3f(0, 1, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -1.25, 0)); contacts.resize(1); contacts[0].pos.setValue(0, -1.25, 0); contacts[0].penetration_depth = 8.75; contacts[0].normal.setValue(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -1.25, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, -1.25, 0)); contacts[0].penetration_depth = 8.75; contacts[0].normal = transform.getQuatRotation().transform(Vec3f(0, -1, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 10.01, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 10.01, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -10.01, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -10.01, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); hs = Plane(Vec3f(0, 0, 1), 0); tf1 = Transform3f(); tf2 = Transform3f(); contacts.resize(1); contacts[0].pos.setValue(0, 0, 0); contacts[0].penetration_depth = 20.0; contacts[0].normal.setValue(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = transform; tf2 = transform; contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, 0)); contacts[0].penetration_depth = 20.0; contacts[0].normal = transform.getQuatRotation().transform(Vec3f(0, 0, -1)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 1.25)); contacts.resize(1); contacts[0].pos.setValue(0, 0, 1.25); contacts[0].penetration_depth = 18.75; contacts[0].normal.setValue(0, 0, 1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 1.25)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, 1.25)); contacts[0].penetration_depth = 18.75; contacts[0].normal = transform.getQuatRotation().transform(Vec3f(0, 0, 1)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -1.25)); contacts.resize(1); contacts[0].pos.setValue(0, 0, -1.25); contacts[0].penetration_depth = 18.75; contacts[0].normal.setValue(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -1.25)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, -1.25)); contacts[0].penetration_depth = 18.75; contacts[0].normal = transform.getQuatRotation().transform(Vec3f(0, 0, -1)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 20.01)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 20.01)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -20.01)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -20.01)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); } BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecapsule) { Capsule s(5, 10); Halfspace hs(Vec3f(1, 0, 0), 0); Transform3f tf1; Transform3f tf2; Transform3f transform; generateRandomTransform(extents, transform); std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); contacts.resize(1); contacts[0].pos.setValue(-2.5, 0, 0); contacts[0].penetration_depth = 5; contacts[0].normal.setValue(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform; contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(-2.5, 0, 0)); contacts[0].penetration_depth = 5; contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(2.5, 0, 0)); contacts.resize(1); contacts[0].pos.setValue(-1.25, 0, 0); contacts[0].penetration_depth = 7.5; contacts[0].normal.setValue(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(2.5, 0, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(-1.25, 0, 0)); contacts[0].penetration_depth = 7.5; contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-2.5, 0, 0)); contacts.resize(1); contacts[0].pos.setValue(-3.75, 0, 0); contacts[0].penetration_depth = 2.5; contacts[0].normal.setValue(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(-3.75, 0, 0)); contacts[0].penetration_depth = 2.5; contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(5.1, 0, 0)); contacts.resize(1); contacts[0].pos.setValue(0.05, 0, 0); contacts[0].penetration_depth = 10.1; contacts[0].normal.setValue(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(5.1, 0, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0.05, 0, 0)); contacts[0].penetration_depth = 10.1; contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-5.1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); hs = Halfspace(Vec3f(0, 1, 0), 0); tf1 = Transform3f(); tf2 = Transform3f(); contacts.resize(1); contacts[0].pos.setValue(0, -2.5, 0); contacts[0].penetration_depth = 5; contacts[0].normal.setValue(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform; contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, -2.5, 0)); contacts[0].penetration_depth = 5; contacts[0].normal = transform.getRotation() * Vec3f(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 2.5, 0)); contacts.resize(1); contacts[0].pos.setValue(0, -1.25, 0); contacts[0].penetration_depth = 7.5; contacts[0].normal.setValue(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 2.5, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, -1.25, 0)); contacts[0].penetration_depth = 7.5; contacts[0].normal = transform.getRotation() * Vec3f(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -2.5, 0)); contacts.resize(1); contacts[0].pos.setValue(0, -3.75, 0); contacts[0].penetration_depth = 2.5; contacts[0].normal.setValue(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -2.5, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, -3.75, 0)); contacts[0].penetration_depth = 2.5; contacts[0].normal = transform.getRotation() * Vec3f(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 5.1, 0)); contacts.resize(1); contacts[0].pos.setValue(0, 0.05, 0); contacts[0].penetration_depth = 10.1; contacts[0].normal.setValue(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 5.1, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0.05, 0)); contacts[0].penetration_depth = 10.1; contacts[0].normal = transform.getRotation() * Vec3f(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -5.1, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -5.1, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); hs = Halfspace(Vec3f(0, 0, 1), 0); tf1 = Transform3f(); tf2 = Transform3f(); contacts.resize(1); contacts[0].pos.setValue(0, 0, -5); contacts[0].penetration_depth = 10; contacts[0].normal.setValue(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform; contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, -5)); contacts[0].penetration_depth = 10; contacts[0].normal = transform.getRotation() * Vec3f(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 2.5)); contacts.resize(1); contacts[0].pos.setValue(0, 0, -3.75); contacts[0].penetration_depth = 12.5; contacts[0].normal.setValue(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 2.5)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, -3.75)); contacts[0].penetration_depth = 12.5; contacts[0].normal = transform.getRotation() * Vec3f(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -2.5)); contacts.resize(1); contacts[0].pos.setValue(0, 0, -6.25); contacts[0].penetration_depth = 7.5; contacts[0].normal.setValue(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -2.5)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, -6.25)); contacts[0].penetration_depth = 7.5; contacts[0].normal = transform.getRotation() * Vec3f(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 10.1)); contacts.resize(1); contacts[0].pos.setValue(0, 0, 0.05); contacts[0].penetration_depth = 20.1; contacts[0].normal.setValue(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 10.1)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, 0.05)); contacts[0].penetration_depth = 20.1; contacts[0].normal = transform.getRotation() * Vec3f(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -10.1)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -10.1)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); } BOOST_AUTO_TEST_CASE(shapeIntersection_planecapsule) { Capsule s(5, 10); Plane hs(Vec3f(1, 0, 0), 0); Transform3f tf1; Transform3f tf2; Transform3f transform; generateRandomTransform(extents, transform); std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); contacts.resize(1); contacts[0].pos.setValue(0, 0, 0); contacts[0].penetration_depth = 5; contacts[0].normal.setValue(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = transform; tf2 = transform; contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, 0)); contacts[0].penetration_depth = 5; contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(2.5, 0, 0)); contacts.resize(1); contacts[0].pos.setValue(2.5, 0, 0); contacts[0].penetration_depth = 2.5; contacts[0].normal.setValue(1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(2.5, 0, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(2.5, 0, 0)); contacts[0].penetration_depth = 2.5; contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-2.5, 0, 0)); contacts.resize(1); contacts[0].pos.setValue(-2.5, 0, 0); contacts[0].penetration_depth = 2.5; contacts[0].normal.setValue(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(-2.5, 0, 0)); contacts[0].penetration_depth = 2.5; contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(5.1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(5.1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-5.1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); hs = Plane(Vec3f(0, 1, 0), 0); tf1 = Transform3f(); tf2 = Transform3f(); contacts.resize(1); contacts[0].pos.setValue(0, 0, 0); contacts[0].penetration_depth = 5; contacts[0].normal.setValue(0, 1, 0); // (0, 1, 0) or (0, -1, 0) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = transform; tf2 = transform; contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, 0)); contacts[0].penetration_depth = 5; contacts[0].normal = transform.getRotation() * Vec3f(0, 1, 0); // (0, 1, 0) or (0, -1, 0) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 2.5, 0)); contacts.resize(1); contacts[0].pos.setValue(0, 2.5, 0); contacts[0].penetration_depth = 2.5; contacts[0].normal.setValue(0, 1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 2.5, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 2.5, 0)); contacts[0].penetration_depth = 2.5; contacts[0].normal = transform.getRotation() * Vec3f(0, 1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -2.5, 0)); contacts.resize(1); contacts[0].pos.setValue(0, -2.5, 0); contacts[0].penetration_depth = 2.5; contacts[0].normal.setValue(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -2.5, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, -2.5, 0)); contacts[0].penetration_depth = 2.5; contacts[0].normal = transform.getRotation() * Vec3f(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 5.1, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 5.1, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -5.1, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -5.1, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); hs = Plane(Vec3f(0, 0, 1), 0); tf1 = Transform3f(); tf2 = Transform3f(); contacts.resize(1); contacts[0].pos.setValue(0, 0, 0); contacts[0].penetration_depth = 10; contacts[0].normal.setValue(0, 0, 1); // (0, 0, 1) or (0, 0, -1) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = transform; tf2 = transform; contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, 0)); contacts[0].penetration_depth = 10; contacts[0].normal = transform.getRotation() * Vec3f(0, 0, 1); // (0, 0, 1) or (0, 0, -1) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 2.5)); contacts.resize(1); contacts[0].pos.setValue(0, 0, 2.5); contacts[0].penetration_depth = 7.5; contacts[0].normal.setValue(0, 0, 1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 2.5)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, 2.5)); contacts[0].penetration_depth = 7.5; contacts[0].normal = transform.getRotation() * Vec3f(0, 0, 1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -2.5)); contacts.resize(1); contacts[0].pos.setValue(0, 0, -2.5); contacts[0].penetration_depth = 7.5; contacts[0].normal.setValue(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -2.5)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, -2.5)); contacts[0].penetration_depth = 7.5; contacts[0].normal = transform.getRotation() * Vec3f(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 10.1)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 10.1)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -10.1)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -10.1)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); } BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecylinder) { Cylinder s(5, 10); Halfspace hs(Vec3f(1, 0, 0), 0); Transform3f tf1; Transform3f tf2; Transform3f transform; generateRandomTransform(extents, transform); std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); contacts.resize(1); contacts[0].pos.setValue(-2.5, 0, 0); contacts[0].penetration_depth = 5; contacts[0].normal.setValue(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform; contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(-2.5, 0, 0)); contacts[0].penetration_depth = 5; contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(2.5, 0, 0)); contacts.resize(1); contacts[0].pos.setValue(-1.25, 0, 0); contacts[0].penetration_depth = 7.5; contacts[0].normal.setValue(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(2.5, 0, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(-1.25, 0, 0)); contacts[0].penetration_depth = 7.5; contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-2.5, 0, 0)); contacts.resize(1); contacts[0].pos.setValue(-3.75, 0, 0); contacts[0].penetration_depth = 2.5; contacts[0].normal.setValue(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(-3.75, 0, 0)); contacts[0].penetration_depth = 2.5; contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(5.1, 0, 0)); contacts.resize(1); contacts[0].pos.setValue(0.05, 0, 0); contacts[0].penetration_depth = 10.1; contacts[0].normal.setValue(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(5.1, 0, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0.05, 0, 0)); contacts[0].penetration_depth = 10.1; contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-5.1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); hs = Halfspace(Vec3f(0, 1, 0), 0); tf1 = Transform3f(); tf2 = Transform3f(); contacts.resize(1); contacts[0].pos.setValue(0, -2.5, 0); contacts[0].penetration_depth = 5; contacts[0].normal.setValue(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform; contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, -2.5, 0)); contacts[0].penetration_depth = 5; contacts[0].normal = transform.getRotation() * Vec3f(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 2.5, 0)); contacts.resize(1); contacts[0].pos.setValue(0, -1.25, 0); contacts[0].penetration_depth = 7.5; contacts[0].normal.setValue(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 2.5, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, -1.25, 0)); contacts[0].penetration_depth = 7.5; contacts[0].normal = transform.getRotation() * Vec3f(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -2.5, 0)); contacts.resize(1); contacts[0].pos.setValue(0, -3.75, 0); contacts[0].penetration_depth = 2.5; contacts[0].normal.setValue(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -2.5, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, -3.75, 0)); contacts[0].penetration_depth = 2.5; contacts[0].normal = transform.getRotation() * Vec3f(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 5.1, 0)); contacts.resize(1); contacts[0].pos.setValue(0, 0.05, 0); contacts[0].penetration_depth = 10.1; contacts[0].normal.setValue(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 5.1, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0.05, 0)); contacts[0].penetration_depth = 10.1; contacts[0].normal = transform.getRotation() * Vec3f(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -5.1, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -5.1, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); hs = Halfspace(Vec3f(0, 0, 1), 0); tf1 = Transform3f(); tf2 = Transform3f(); contacts.resize(1); contacts[0].pos.setValue(0, 0, -2.5); contacts[0].penetration_depth = 5; contacts[0].normal.setValue(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform; contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, -2.5)); contacts[0].penetration_depth = 5; contacts[0].normal = transform.getRotation() * Vec3f(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 2.5)); contacts.resize(1); contacts[0].pos.setValue(0, 0, -1.25); contacts[0].penetration_depth = 7.5; contacts[0].normal.setValue(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 2.5)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, -1.25)); contacts[0].penetration_depth = 7.5; contacts[0].normal = transform.getRotation() * Vec3f(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -2.5)); contacts.resize(1); contacts[0].pos.setValue(0, 0, -3.75); contacts[0].penetration_depth = 2.5; contacts[0].normal.setValue(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -2.5)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, -3.75)); contacts[0].penetration_depth = 2.5; contacts[0].normal = transform.getRotation() * Vec3f(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 5.1)); contacts.resize(1); contacts[0].pos.setValue(0, 0, 0.05); contacts[0].penetration_depth = 10.1; contacts[0].normal.setValue(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 5.1)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, 0.05)); contacts[0].penetration_depth = 10.1; contacts[0].normal = transform.getRotation() * Vec3f(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -5.1)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -5.1)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); } BOOST_AUTO_TEST_CASE(shapeIntersection_planecylinder) { Cylinder s(5, 10); Plane hs(Vec3f(1, 0, 0), 0); Transform3f tf1; Transform3f tf2; Transform3f transform; generateRandomTransform(extents, transform); std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); contacts.resize(1); contacts[0].pos.setValue(0, 0, 0); contacts[0].penetration_depth = 5; contacts[0].normal.setValue(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = transform; tf2 = transform; contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, 0)); contacts[0].penetration_depth = 5; contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(2.5, 0, 0)); contacts.resize(1); contacts[0].pos.setValue(2.5, 0, 0); contacts[0].penetration_depth = 2.5; contacts[0].normal.setValue(1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(2.5, 0, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(2.5, 0, 0)); contacts[0].penetration_depth = 2.5; contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-2.5, 0, 0)); contacts.resize(1); contacts[0].pos.setValue(-2.5, 0, 0); contacts[0].penetration_depth = 2.5; contacts[0].normal.setValue(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(-2.5, 0, 0)); contacts[0].penetration_depth = 2.5; contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(5.1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(5.1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-5.1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); hs = Plane(Vec3f(0, 1, 0), 0); tf1 = Transform3f(); tf2 = Transform3f(); contacts.resize(1); contacts[0].pos.setValue(0, 0, 0); contacts[0].penetration_depth = 5; contacts[0].normal.setValue(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = transform; tf2 = transform; contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, 0)); contacts[0].penetration_depth = 5; contacts[0].normal = transform.getRotation() * Vec3f(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 2.5, 0)); contacts.resize(1); contacts[0].pos.setValue(0, 2.5, 0); contacts[0].penetration_depth = 2.5; contacts[0].normal.setValue(0, 1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 2.5, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 2.5, 0)); contacts[0].penetration_depth = 2.5; contacts[0].normal = transform.getRotation() * Vec3f(0, 1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -2.5, 0)); contacts.resize(1); contacts[0].pos.setValue(0, -2.5, 0); contacts[0].penetration_depth = 2.5; contacts[0].normal.setValue(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -2.5, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, -2.5, 0)); contacts[0].penetration_depth = 2.5; contacts[0].normal = transform.getRotation() * Vec3f(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 5.1, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 5.1, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -5.1, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -5.1, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); hs = Plane(Vec3f(0, 0, 1), 0); tf1 = Transform3f(); tf2 = Transform3f(); contacts.resize(1); contacts[0].pos.setValue(0, 0, 0); contacts[0].penetration_depth = 5; contacts[0].normal.setValue(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = transform; tf2 = transform; contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, 0)); contacts[0].penetration_depth = 5; contacts[0].normal = transform.getRotation() * Vec3f(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 2.5)); contacts.resize(1); contacts[0].pos.setValue(0, 0, 2.5); contacts[0].penetration_depth = 2.5; contacts[0].normal.setValue(0, 0, 1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 2.5)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, 2.5)); contacts[0].penetration_depth = 2.5; contacts[0].normal = transform.getRotation() * Vec3f(0, 0, 1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -2.5)); contacts.resize(1); contacts[0].pos.setValue(0, 0, -2.5); contacts[0].penetration_depth = 2.5; contacts[0].normal.setValue(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -2.5)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, -2.5)); contacts[0].penetration_depth = 2.5; contacts[0].normal = transform.getRotation() * Vec3f(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 10.1)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 10.1)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -10.1)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -10.1)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); } BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacecone) { Cone s(5, 10); Halfspace hs(Vec3f(1, 0, 0), 0); Transform3f tf1; Transform3f tf2; Transform3f transform; generateRandomTransform(extents, transform); std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); contacts.resize(1); contacts[0].pos.setValue(-2.5, 0, -5); contacts[0].penetration_depth = 5; contacts[0].normal.setValue(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform; contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(-2.5, 0, -5)); contacts[0].penetration_depth = 5; contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(2.5, 0, 0)); contacts.resize(1); contacts[0].pos.setValue(-1.25, 0, -5); contacts[0].penetration_depth = 7.5; contacts[0].normal.setValue(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(2.5, 0, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(-1.25, 0, -5)); contacts[0].penetration_depth = 7.5; contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-2.5, 0, 0)); contacts.resize(1); contacts[0].pos.setValue(-3.75, 0, -5); contacts[0].penetration_depth = 2.5; contacts[0].normal.setValue(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(-3.75, 0, -5)); contacts[0].penetration_depth = 2.5; contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(5.1, 0, 0)); contacts.resize(1); contacts[0].pos.setValue(0.05, 0, -5); contacts[0].penetration_depth = 10.1; contacts[0].normal.setValue(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(5.1, 0, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0.05, 0, -5)); contacts[0].penetration_depth = 10.1; contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-5.1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); hs = Halfspace(Vec3f(0, 1, 0), 0); tf1 = Transform3f(); tf2 = Transform3f(); contacts.resize(1); contacts[0].pos.setValue(0, -2.5, -5); contacts[0].penetration_depth = 5; contacts[0].normal.setValue(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform; contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, -2.5, -5)); contacts[0].penetration_depth = 5; contacts[0].normal = transform.getRotation() * Vec3f(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 2.5, 0)); contacts.resize(1); contacts[0].pos.setValue(0, -1.25, -5); contacts[0].penetration_depth = 7.5; contacts[0].normal.setValue(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 2.5, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, -1.25, -5)); contacts[0].penetration_depth = 7.5; contacts[0].normal = transform.getRotation() * Vec3f(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -2.5, 0)); contacts.resize(1); contacts[0].pos.setValue(0, -3.75, -5); contacts[0].penetration_depth = 2.5; contacts[0].normal.setValue(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -2.5, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, -3.75, -5)); contacts[0].penetration_depth = 2.5; contacts[0].normal = transform.getRotation() * Vec3f(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 5.1, 0)); contacts.resize(1); contacts[0].pos.setValue(0, 0.05, -5); contacts[0].penetration_depth = 10.1; contacts[0].normal.setValue(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 5.1, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0.05, -5)); contacts[0].penetration_depth = 10.1; contacts[0].normal = transform.getRotation() * Vec3f(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -5.1, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -5.1, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); hs = Halfspace(Vec3f(0, 0, 1), 0); tf1 = Transform3f(); tf2 = Transform3f(); contacts.resize(1); contacts[0].pos.setValue(0, 0, -2.5); contacts[0].penetration_depth = 5; contacts[0].normal.setValue(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform; contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, -2.5)); contacts[0].penetration_depth = 5; contacts[0].normal = transform.getRotation() * Vec3f(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 2.5)); contacts.resize(1); contacts[0].pos.setValue(0, 0, -1.25); contacts[0].penetration_depth = 7.5; contacts[0].normal.setValue(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 2.5)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, -1.25)); contacts[0].penetration_depth = 7.5; contacts[0].normal = transform.getRotation() * Vec3f(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -2.5)); contacts.resize(1); contacts[0].pos.setValue(0, 0, -3.75); contacts[0].penetration_depth= 2.5; contacts[0].normal.setValue(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -2.5)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, -3.75)); contacts[0].penetration_depth = 2.5; contacts[0].normal = transform.getRotation() * Vec3f(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 5.1)); contacts.resize(1); contacts[0].pos.setValue(0, 0, 0.05); contacts[0].penetration_depth = 10.1; contacts[0].normal.setValue(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 5.1)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, 0.05)); contacts[0].penetration_depth = 10.1; contacts[0].normal = transform.getRotation() * Vec3f(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -5.1)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -5.1)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); } BOOST_AUTO_TEST_CASE(shapeIntersection_planecone) { Cone s(5, 10); Plane hs(Vec3f(1, 0, 0), 0); Transform3f tf1; Transform3f tf2; Transform3f transform; generateRandomTransform(extents, transform); std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); contacts.resize(1); contacts[0].pos.setValue(0, 0, 0); contacts[0].penetration_depth = 5; contacts[0].normal.setValue(1, 0, 0); // (1, 0, 0) or (-1, 0, 0) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = transform; tf2 = transform; contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, 0)); contacts[0].penetration_depth = 5; contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); // (1, 0, 0) or (-1, 0, 0) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(2.5, 0, 0)); contacts.resize(1); contacts[0].pos.setValue(2.5, 0, -2.5); contacts[0].penetration_depth = 2.5; contacts[0].normal.setValue(1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(2.5, 0, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(2.5, 0, -2.5)); contacts[0].penetration_depth = 2.5; contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-2.5, 0, 0)); contacts.resize(1); contacts[0].pos.setValue(-2.5, 0, -2.5); contacts[0].penetration_depth = 2.5; contacts[0].normal.setValue(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-2.5, 0, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(-2.5, 0, -2.5)); contacts[0].penetration_depth = 2.5; contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(5.1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(5.1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-5.1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-5.1, 0, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); hs = Plane(Vec3f(0, 1, 0), 0); tf1 = Transform3f(); tf2 = Transform3f(); contacts.resize(1); contacts[0].pos.setValue(0, 0, 0); contacts[0].penetration_depth = 5; contacts[0].normal.setValue(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = transform; tf2 = transform; contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, 0)); contacts[0].penetration_depth = 5; contacts[0].normal = transform.getRotation() * Vec3f(0, 1, 0); // (1, 0, 0) or (-1, 0, 0) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 2.5, 0)); contacts.resize(1); contacts[0].pos.setValue(0, 2.5, -2.5); contacts[0].penetration_depth = 2.5; contacts[0].normal.setValue(0, 1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 2.5, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 2.5, -2.5)); contacts[0].penetration_depth = 2.5; contacts[0].normal = transform.getRotation() * Vec3f(0, 1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -2.5, 0)); contacts.resize(1); contacts[0].pos.setValue(0, -2.5, -2.5); contacts[0].penetration_depth = 2.5; contacts[0].normal.setValue(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -2.5, 0)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, -2.5, -2.5)); contacts[0].penetration_depth = 2.5; contacts[0].normal = transform.getRotation() * Vec3f(0, -1, 0); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 5.1, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 5.1, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, -5.1, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, -5.1, 0)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); hs = Plane(Vec3f(0, 0, 1), 0); tf1 = Transform3f(); tf2 = Transform3f(); contacts.resize(1); contacts[0].pos.setValue(0, 0, 0); contacts[0].penetration_depth = 5; contacts[0].normal.setValue(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = transform; tf2 = transform; contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, 0)); contacts[0].penetration_depth = 5; contacts[0].normal = transform.getRotation() * Vec3f(0, 0, 1); // (1, 0, 0) or (-1, 0, 0) testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 2.5)); contacts.resize(1); contacts[0].pos.setValue(0, 0, 2.5); contacts[0].penetration_depth = 2.5; contacts[0].normal.setValue(0, 0, 1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 2.5)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, 2.5)); contacts[0].penetration_depth = 2.5; contacts[0].normal = transform.getRotation() * Vec3f(0, 0, 1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -2.5)); contacts.resize(1); contacts[0].pos.setValue(0, 0, -2.5); contacts[0].penetration_depth = 2.5; contacts[0].normal.setValue(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -2.5)); contacts.resize(1); contacts[0].pos = transform.transform(Vec3f(0, 0, -2.5)); contacts[0].penetration_depth = 2.5; contacts[0].normal = transform.getRotation() * Vec3f(0, 0, -1); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 10.1)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 10.1)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, -10.1)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, -10.1)); testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false); } // Shape distance test coverage (libccd) // // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | box | O | O | | | | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | sphere |/////| O | | | | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | ellipsoid |/////|////////| O | | | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | capsule |/////|////////|///////////| | | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | cone |/////|////////|///////////|/////////| O | O | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | cylinder |/////|////////|///////////|/////////|//////| O | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | plane |/////|////////|///////////|/////////|//////|//////////| | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | half-space |/////|////////|///////////|/////////|//////|//////////|///////| | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ BOOST_AUTO_TEST_CASE(shapeDistance_spheresphere) { Sphere s1(20); Sphere s2(10); Transform3f transform; //generateRandomTransform(extents, transform); bool res; FCL_REAL dist = -1; Vec3f closest_p1, closest_p2; res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(0, 40, 0)), &dist, &closest_p1, &closest_p2); BOOST_CHECK(fabs(dist - 10) < 0.001); BOOST_CHECK(res); res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(30.1, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)), &dist); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); res = solver1.shapeDistance(s1, Transform3f(Vec3f(40, 0, 0)), s2, Transform3f(), &dist); BOOST_CHECK(fabs(dist - 10) < 0.001); BOOST_CHECK(res); res = solver1.shapeDistance(s1, Transform3f(Vec3f(30.1, 0, 0)), s2, Transform3f(), &dist); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); res = solver1.shapeDistance(s1, Transform3f(Vec3f(29.9, 0, 0)), s2, Transform3f(), &dist); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist); // this is one problem: the precise is low sometimes BOOST_CHECK(fabs(dist - 10) < 0.1); BOOST_CHECK(res); res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(30.1, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 0.1) < 0.06); BOOST_CHECK(res); res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)), &dist); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); res = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(40, 0, 0)), s2, transform, &dist); BOOST_CHECK(fabs(dist - 10) < 0.1); BOOST_CHECK(res); res = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(30.1, 0, 0)), s2, transform, &dist); BOOST_CHECK(fabs(dist - 0.1) < 0.1); BOOST_CHECK(res); res = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(29.9, 0, 0)), s2, transform, &dist); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); } BOOST_AUTO_TEST_CASE(shapeDistance_boxbox) { Box s1(20, 40, 50); Box s2(10, 10, 10); Vec3f closest_p1, closest_p2; Transform3f transform; //generateRandomTransform(extents, transform); bool res; FCL_REAL dist; res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); res = solver1.shapeDistance(s1, transform, s2, transform, &dist); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); res = solver1.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &dist, &closest_p1, &closest_p2); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); res = solver1.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(20.1, 0, 0)), &dist, &closest_p1, &closest_p2); BOOST_CHECK(fabs(dist - 10.1) < 0.001); BOOST_CHECK(res); res = solver1.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(0, 20.2, 0)), &dist, &closest_p1, &closest_p2); BOOST_CHECK(fabs(dist - 10.2) < 0.001); BOOST_CHECK(res); res = solver1.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 10.1, 0)), &dist, &closest_p1, &closest_p2); BOOST_CHECK(fabs(dist - 0.1 * 1.414) < 0.001); BOOST_CHECK(res); res = solver2.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &dist, &closest_p1, &closest_p2); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); res = solver2.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(20.1, 0, 0)), &dist, &closest_p1, &closest_p2); BOOST_CHECK(fabs(dist - 10.1) < 0.001); BOOST_CHECK(res); res = solver2.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(0, 20.1, 0)), &dist, &closest_p1, &closest_p2); BOOST_CHECK(fabs(dist - 10.1) < 0.001); BOOST_CHECK(res); res = solver2.shapeDistance(s2, Transform3f(), s2, Transform3f(Vec3f(10.1, 10.1, 0)), &dist, &closest_p1, &closest_p2); BOOST_CHECK(fabs(dist - 0.1 * 1.414) < 0.001); BOOST_CHECK(res); res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(15.1, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(20, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 5) < 0.001); BOOST_CHECK(res); res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(20, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 5) < 0.001); BOOST_CHECK(res); } BOOST_AUTO_TEST_CASE(shapeDistance_boxsphere) { Sphere s1(20); Box s2(5, 5, 5); Transform3f transform; generateRandomTransform(extents, transform); bool res; FCL_REAL dist; res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); res = solver1.shapeDistance(s1, transform, s2, transform, &dist); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(22.6, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(22.6, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 0.1) < 0.05); BOOST_CHECK(res); res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 17.5) < 0.001); BOOST_CHECK(res); res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 17.5) < 0.001); BOOST_CHECK(res); } BOOST_AUTO_TEST_CASE(shapeDistance_cylindercylinder) { Cylinder s1(5, 10); Cylinder s2(5, 10); Transform3f transform; generateRandomTransform(extents, transform); bool res; FCL_REAL dist; res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); res = solver1.shapeDistance(s1, transform, s2, transform, &dist); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 30) < 0.001); BOOST_CHECK(res); res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 30) < 0.001); BOOST_CHECK(res); } BOOST_AUTO_TEST_CASE(shapeDistance_conecone) { Cone s1(5, 10); Cone s2(5, 10); Transform3f transform; generateRandomTransform(extents, transform); bool res; FCL_REAL dist; res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); res = solver1.shapeDistance(s1, transform, s2, transform, &dist); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 40)), &dist); BOOST_CHECK(fabs(dist - 30) < 1); BOOST_CHECK(res); res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 40)), &dist); BOOST_CHECK(fabs(dist - 30) < 1); BOOST_CHECK(res); } BOOST_AUTO_TEST_CASE(shapeDistance_conecylinder) { Cylinder s1(5, 10); Cone s2(5, 10); Transform3f transform; generateRandomTransform(extents, transform); bool res; FCL_REAL dist; res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); res = solver1.shapeDistance(s1, transform, s2, transform, &dist); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 0.1) < 0.01); BOOST_CHECK(res); res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 0.1) < 0.02); BOOST_CHECK(res); res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 30) < 0.01); BOOST_CHECK(res); res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 30) < 0.1); BOOST_CHECK(res); } BOOST_AUTO_TEST_CASE(shapeDistance_ellipsoidellipsoid) { Ellipsoid s1(20, 40, 50); Ellipsoid s2(10, 10, 10); Transform3f transform; generateRandomTransform(extents, transform); bool res; FCL_REAL dist = -1; Vec3f closest_p1, closest_p2; res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist, &closest_p1, &closest_p2); BOOST_CHECK(fabs(dist - 10) < 0.001); BOOST_CHECK(res); res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(30.1, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); res = solver1.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)), &dist); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); res = solver1.shapeDistance(s1, Transform3f(Vec3f(40, 0, 0)), s2, Transform3f(), &dist); BOOST_CHECK(fabs(dist - 10) < 0.001); BOOST_CHECK(res); res = solver1.shapeDistance(s1, Transform3f(Vec3f(30.1, 0, 0)), s2, Transform3f(), &dist); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); res = solver1.shapeDistance(s1, Transform3f(Vec3f(29.9, 0, 0)), s2, Transform3f(), &dist); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 10) < 0.001); BOOST_CHECK(res); res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(30.1, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); res = solver1.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)), &dist); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); res = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(40, 0, 0)), s2, transform, &dist); BOOST_CHECK(fabs(dist - 10) < 0.001); BOOST_CHECK(res); res = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(30.1, 0, 0)), s2, transform, &dist); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); res = solver1.shapeDistance(s1, transform * Transform3f(Vec3f(29.9, 0, 0)), s2, transform, &dist); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); } // Shape intersection test coverage (built-in GJK) // // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | box | O | O | | | | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | sphere |/////| O | | | | | | | O | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | ellipsoid |/////|////////| O | | | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | capsule |/////|////////|///////////| | | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | cone |/////|////////|///////////|/////////| O | O | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | cylinder |/////|////////|///////////|/////////|//////| O | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | plane |/////|////////|///////////|/////////|//////|//////////| | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | half-space |/////|////////|///////////|/////////|//////|//////////|///////| | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheresphere) { Sphere s1(20); Sphere s2(10); Transform3f tf1; Transform3f tf2; Transform3f transform; generateRandomTransform(extents, transform); std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(Vec3f(40, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(40, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(30, 0, 0)); contacts.resize(1); contacts[0].normal.setValue(1, 0, 0); contacts[0].pos.setValue(20, 0, 0); contacts[0].penetration_depth = 0.0; testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(30.01, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(30.01, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(29.9, 0, 0)); contacts.resize(1); contacts[0].normal.setValue(1, 0, 0); contacts[0].pos.setValue(20.0 - 0.1 * 20.0/(20.0 + 10.0), 0, 0); contacts[0].penetration_depth = 0.1; testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(29.9, 0, 0)); contacts.resize(1); contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); contacts[0].pos = transform.transform(Vec3f(20.0 - 0.1 * 20.0/(20.0 + 10.0), 0, 0)); contacts[0].penetration_depth = 0.1; testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(); contacts.resize(1); contacts[0].normal.setZero(); // If the centers of two sphere are at the same position, the normal is (0, 0, 0) contacts[0].pos.setZero(); contacts[0].penetration_depth = 20.0 + 10.0; testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts); tf1 = transform; tf2 = transform; contacts.resize(1); contacts[0].normal.setZero(); // If the centers of two sphere are at the same position, the normal is (0, 0, 0) contacts[0].pos = transform.transform(Vec3f()); contacts[0].penetration_depth = 20.0 + 10.0; testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-29.9, 0, 0)); contacts.resize(1); contacts[0].normal.setValue(-1, 0, 0); contacts[0].pos.setValue(-20.0 + 0.1 * 20.0/(20.0 + 10.0), 0, 0); contacts[0].penetration_depth = 0.1; testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-29.9, 0, 0)); contacts.resize(1); contacts[0].normal = transform.getRotation() * Vec3f(-1, 0, 0); contacts[0].pos = transform.transform(Vec3f(-20.0 + 0.1 * 20.0/(20.0 + 10.0), 0, 0)); contacts[0].penetration_depth = 0.1; testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-30.0, 0, 0)); contacts.resize(1); contacts[0].normal.setValue(-1, 0, 0); contacts[0].pos.setValue(-20, 0, 0); contacts[0].penetration_depth = 0.0; testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-30.01, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-30.01, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); } BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_boxbox) { Box s1(20, 40, 50); Box s2(10, 10, 10); Transform3f tf1; Transform3f tf2; Transform3f transform; generateRandomTransform(extents, transform); std::vector contacts; Quaternion3f q; q.fromAxisAngle(Vec3f(0, 0, 1), (FCL_REAL)3.140 / 6); tf1 = Transform3f(); tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0). contacts.resize(4); contacts[0].normal.setValue(1, 0, 0); contacts[1].normal.setValue(1, 0, 0); contacts[2].normal.setValue(1, 0, 0); contacts[3].normal.setValue(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0). contacts.resize(4); contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); contacts[1].normal = transform.getRotation() * Vec3f(1, 0, 0); contacts[2].normal = transform.getRotation() * Vec3f(1, 0, 0); contacts[3].normal = transform.getRotation() * Vec3f(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(15, 0, 0)); contacts.resize(4); contacts[0].normal = Vec3f(1, 0, 0); contacts[1].normal = Vec3f(1, 0, 0); contacts[2].normal = Vec3f(1, 0, 0); contacts[3].normal = Vec3f(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true); tf1 = transform; tf2 = transform * Transform3f(Vec3f(15.01, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); tf1 = Transform3f(); tf2 = Transform3f(q); contacts.resize(4); contacts[0].normal = Vec3f(1, 0, 0); contacts[1].normal = Vec3f(1, 0, 0); contacts[2].normal = Vec3f(1, 0, 0); contacts[3].normal = Vec3f(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true); tf1 = transform; tf2 = transform * Transform3f(q); contacts.resize(4); contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); contacts[1].normal = transform.getRotation() * Vec3f(1, 0, 0); contacts[2].normal = transform.getRotation() * Vec3f(1, 0, 0); contacts[3].normal = transform.getRotation() * Vec3f(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true); } BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spherebox) { Sphere s1(20); Box s2(5, 5, 5); Transform3f tf1; Transform3f tf2; Transform3f transform; generateRandomTransform(extents, transform); std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(22.5, 0, 0)); contacts.resize(1); contacts[0].normal.setValue(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true, false, 1e-7); // built-in GJK solver requires larger tolerance than libccd tf1 = transform; tf2 = transform * Transform3f(Vec3f(22.51, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(22.4, 0, 0)); contacts.resize(1); contacts[0].normal.setValue(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true, false, 1e-2); // built-in GJK solver requires larger tolerance than libccd tf1 = transform; tf2 = transform * Transform3f(Vec3f(22.4, 0, 0)); contacts.resize(1); // contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); // built-in GJK solver returns incorrect normal. } BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spherecapsule) { Sphere s1(20); Capsule s2(5, 10); Transform3f tf1; Transform3f tf2; Transform3f transform; generateRandomTransform(extents, transform); std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(24.9, 0, 0)); contacts.resize(1); contacts[0].normal.setValue(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true); tf1 = transform; tf2 = transform * Transform3f(Vec3f(24.9, 0, 0)); contacts.resize(1); contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(25, 0, 0)); contacts.resize(1); contacts[0].normal.setValue(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true); tf1 = transform; tf2 = transform * Transform3f(Vec3f(25.1, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); } BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_cylindercylinder) { Cylinder s1(5, 10); Cylinder s2(5, 10); Transform3f tf1; Transform3f tf2; Transform3f transform; generateRandomTransform(extents, transform); std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(9.9, 0, 0)); contacts.resize(1); contacts[0].normal.setValue(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true, false, 3e-1); // built-in GJK solver requires larger tolerance than libccd tf1 = transform; tf2 = transform * Transform3f(Vec3f(9.9, 0, 0)); contacts.resize(1); // contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); // built-in GJK solver returns incorrect normal. tf1 = Transform3f(); tf2 = Transform3f(Vec3f(10, 0, 0)); contacts.resize(1); contacts[0].normal.setValue(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true); tf1 = transform; tf2 = transform * Transform3f(Vec3f(10.01, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); } BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_conecone) { Cone s1(5, 10); Cone s2(5, 10); Transform3f tf1; Transform3f tf2; Transform3f transform; generateRandomTransform(extents, transform); std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(9.9, 0, 0)); contacts.resize(1); contacts[0].normal.setValue(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true, false, 5.7e-1); // built-in GJK solver requires larger tolerance than libccd tf1 = transform; tf2 = transform * Transform3f(Vec3f(9.9, 0, 0)); contacts.resize(1); // contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); // built-in GJK solver returns incorrect normal. tf1 = Transform3f(); tf2 = Transform3f(Vec3f(10.1, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(10.1, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 9.9)); contacts.resize(1); contacts[0].normal.setValue(0, 0, 1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 9.9)); contacts.resize(1); // contacts[0].normal = transform.getRotation() * Vec3f(0, 0, 1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); // built-in GJK solver returns incorrect normal. } BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_cylindercone) { Cylinder s1(5, 10); Cone s2(5, 10); Transform3f tf1; Transform3f tf2; Transform3f transform; generateRandomTransform(extents, transform); std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(); // TODO: Need convention for normal when the centers of two objects are at same position. contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = transform; tf2 = transform; // TODO: Need convention for normal when the centers of two objects are at same position. contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(9.9, 0, 0)); contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(9.9, 0, 0)); contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(10, 0, 0)); contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(10, 0, 0)); contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 9.9)); contacts.resize(1); contacts[0].normal.setValue(0, 0, 1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 9.9)); contacts.resize(1); // contacts[0].normal = transform.getRotation() * Vec3f(1, 0, 0); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); // built-in GJK solver returns incorrect normal. tf1 = Transform3f(); tf2 = Transform3f(Vec3f(0, 0, 10)); contacts.resize(1); contacts[0].normal.setValue(0, 0, 1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true); tf1 = transform; tf2 = transform * Transform3f(Vec3f(0, 0, 10.1)); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); } BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_ellipsoidellipsoid) { Ellipsoid s1(20, 40, 50); Ellipsoid s2(10, 10, 10); Transform3f tf1; Transform3f tf2; Transform3f transform; generateRandomTransform(extents, transform); Transform3f identity; std::vector contacts; tf1 = Transform3f(); tf2 = Transform3f(Vec3f(40, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(40, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(30, 0, 0)); contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(30.01, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(29.99, 0, 0)); contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(29.9, 0, 0)); contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = Transform3f(); tf2 = Transform3f(); contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = transform; tf2 = transform; contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-29.99, 0, 0)); contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-29.99, 0, 0)); contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = Transform3f(); tf2 = Transform3f(Vec3f(-30, 0, 0)); contacts.resize(1); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false); tf1 = transform; tf2 = transform * Transform3f(Vec3f(-30.01, 0, 0)); testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false); } BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheretriangle) { Sphere s(10); Vec3f t[3]; t[0].setValue(20, 0, 0); t[1].setValue(-20, 0, 0); t[2].setValue(0, 20, 0); Transform3f transform; generateRandomTransform(extents, transform); // Vec3f point; // FCL_REAL depth; Vec3f normal; bool res; res = solver2.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, NULL); BOOST_CHECK(res); res = solver2.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL); BOOST_CHECK(res); t[0].setValue(30, 0, 0); t[1].setValue(9.9, -20, 0); t[2].setValue(9.9, 20, 0); res = solver2.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, NULL); BOOST_CHECK(res); res = solver2.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL); BOOST_CHECK(res); res = solver2.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, &normal); BOOST_CHECK(res); BOOST_CHECK(normal.equal(Vec3f(1, 0, 0), 1e-9)); res = solver2.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, &normal); BOOST_CHECK(res); BOOST_CHECK(normal.equal(transform.getRotation() * Vec3f(1, 0, 0), 1e-9)); } BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_halfspacetriangle) { Halfspace hs(Vec3f(1, 0, 0), 0); Vec3f t[3]; t[0].setValue(20, 0, 0); t[1].setValue(-20, 0, 0); t[2].setValue(0, 20, 0); Transform3f transform; generateRandomTransform(extents, transform); // Vec3f point; // FCL_REAL depth; Vec3f normal; bool res; res = solver2.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, NULL); BOOST_CHECK(res); res = solver2.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL); BOOST_CHECK(res); t[0].setValue(20, 0, 0); t[1].setValue(-0.1, -20, 0); t[2].setValue(-0.1, 20, 0); res = solver2.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, NULL); BOOST_CHECK(res); res = solver2.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL); BOOST_CHECK(res); res = solver2.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, &normal); BOOST_CHECK(res); BOOST_CHECK(normal.equal(Vec3f(1, 0, 0), 1e-9)); res = solver2.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, &normal); BOOST_CHECK(res); BOOST_CHECK(normal.equal(transform.getRotation() * Vec3f(1, 0, 0), 1e-9)); } BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_planetriangle) { Plane hs(Vec3f(1, 0, 0), 0); Vec3f t[3]; t[0].setValue(20, 0, 0); t[1].setValue(-20, 0, 0); t[2].setValue(0, 20, 0); Transform3f transform; generateRandomTransform(extents, transform); // Vec3f point; // FCL_REAL depth; Vec3f normal; bool res; res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, NULL); BOOST_CHECK(res); res = solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL); BOOST_CHECK(res); t[0].setValue(20, 0, 0); t[1].setValue(-0.1, -20, 0); t[2].setValue(-0.1, 20, 0); res = solver2.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, NULL); BOOST_CHECK(res); res = solver2.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, NULL); BOOST_CHECK(res); res = solver2.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, &normal); BOOST_CHECK(res); BOOST_CHECK(normal.equal(Vec3f(1, 0, 0), 1e-9)); res = solver2.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, &normal); BOOST_CHECK(res); BOOST_CHECK(normal.equal(transform.getRotation() * Vec3f(1, 0, 0), 1e-9)); } // Shape distance test coverage (built-in GJK) // // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | box | O | O | | | | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | sphere |/////| O | | | | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | ellipsoid |/////|////////| O | | | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | capsule |/////|////////|///////////| | | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | cone |/////|////////|///////////|/////////| O | | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | cylinder |/////|////////|///////////|/////////|//////| O | | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | plane |/////|////////|///////////|/////////|//////|//////////| | | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | half-space |/////|////////|///////////|/////////|//////|//////////|///////| | | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ // | triangle |/////|////////|///////////|/////////|//////|//////////|///////|////////////| | // +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+ BOOST_AUTO_TEST_CASE(shapeDistanceGJK_spheresphere) { Sphere s1(20); Sphere s2(10); Transform3f transform; generateRandomTransform(extents, transform); bool res; FCL_REAL dist = -1; res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 10) < 0.001); BOOST_CHECK(res); res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(30.1, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)), &dist); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); res = solver2.shapeDistance(s1, Transform3f(Vec3f(40, 0, 0)), s2, Transform3f(), &dist); BOOST_CHECK(fabs(dist - 10) < 0.001); BOOST_CHECK(res); res = solver2.shapeDistance(s1, Transform3f(Vec3f(30.1, 0, 0)), s2, Transform3f(), &dist); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); res = solver2.shapeDistance(s1, Transform3f(Vec3f(29.9, 0, 0)), s2, Transform3f(), &dist); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 10) < 0.001); BOOST_CHECK(res); res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(30.1, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)), &dist); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); res = solver2.shapeDistance(s1, transform * Transform3f(Vec3f(40, 0, 0)), s2, transform, &dist); BOOST_CHECK(fabs(dist - 10) < 0.001); BOOST_CHECK(res); res = solver2.shapeDistance(s1, transform * Transform3f(Vec3f(30.1, 0, 0)), s2, transform, &dist); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); res = solver2.shapeDistance(s1, transform * Transform3f(Vec3f(29.9, 0, 0)), s2, transform, &dist); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); } BOOST_AUTO_TEST_CASE(shapeDistanceGJK_boxbox) { Box s1(20, 40, 50); Box s2(10, 10, 10); Transform3f transform; generateRandomTransform(extents, transform); bool res; FCL_REAL dist; res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); res = solver2.shapeDistance(s1, transform, s2, transform, &dist); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(15.1, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(15.1, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(20, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 5) < 0.001); BOOST_CHECK(res); res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(20, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 5) < 0.001); BOOST_CHECK(res); } BOOST_AUTO_TEST_CASE(shapeDistanceGJK_boxsphere) { Sphere s1(20); Box s2(5, 5, 5); Transform3f transform; generateRandomTransform(extents, transform); bool res; FCL_REAL dist; res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); res = solver2.shapeDistance(s1, transform, s2, transform, &dist); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(22.6, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 0.1) < 0.01); BOOST_CHECK(res); res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(22.6, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 0.1) < 0.01); BOOST_CHECK(res); res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 17.5) < 0.001); BOOST_CHECK(res); res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 17.5) < 0.001); BOOST_CHECK(res); } BOOST_AUTO_TEST_CASE(shapeDistanceGJK_cylindercylinder) { Cylinder s1(5, 10); Cylinder s2(5, 10); Transform3f transform; generateRandomTransform(extents, transform); bool res; FCL_REAL dist; res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); res = solver2.shapeDistance(s1, transform, s2, transform, &dist); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 30) < 0.001); BOOST_CHECK(res); res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 30) < 0.001); BOOST_CHECK(res); } BOOST_AUTO_TEST_CASE(shapeDistanceGJK_conecone) { Cone s1(5, 10); Cone s2(5, 10); Transform3f transform; generateRandomTransform(extents, transform); bool res; FCL_REAL dist; res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(), &dist); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); res = solver2.shapeDistance(s1, transform, s2, transform, &dist); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(10.1, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(10.1, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(0, 0, 40)), &dist); BOOST_CHECK(fabs(dist - 30) < 0.001); BOOST_CHECK(res); res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(0, 0, 40)), &dist); BOOST_CHECK(fabs(dist - 30) < 0.001); BOOST_CHECK(res); } BOOST_AUTO_TEST_CASE(shapeDistanceGJK_ellipsoidellipsoid) { Ellipsoid s1(20, 40, 50); Ellipsoid s2(10, 10, 10); Transform3f transform; generateRandomTransform(extents, transform); bool res; FCL_REAL dist = -1; res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(40, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 10) < 0.001); BOOST_CHECK(res); res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(30.1, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); res = solver2.shapeDistance(s1, Transform3f(), s2, Transform3f(Vec3f(29.9, 0, 0)), &dist); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); res = solver2.shapeDistance(s1, Transform3f(Vec3f(40, 0, 0)), s2, Transform3f(), &dist); BOOST_CHECK(fabs(dist - 10) < 0.001); BOOST_CHECK(res); res = solver2.shapeDistance(s1, Transform3f(Vec3f(30.1, 0, 0)), s2, Transform3f(), &dist); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); res = solver2.shapeDistance(s1, Transform3f(Vec3f(29.9, 0, 0)), s2, Transform3f(), &dist); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(40, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 10) < 0.001); BOOST_CHECK(res); res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(30.1, 0, 0)), &dist); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); res = solver2.shapeDistance(s1, transform, s2, transform * Transform3f(Vec3f(29.9, 0, 0)), &dist); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); res = solver2.shapeDistance(s1, transform * Transform3f(Vec3f(40, 0, 0)), s2, transform, &dist); BOOST_CHECK(fabs(dist - 10) < 0.001); BOOST_CHECK(res); res = solver2.shapeDistance(s1, transform * Transform3f(Vec3f(30.1, 0, 0)), s2, transform, &dist); BOOST_CHECK(fabs(dist - 0.1) < 0.001); BOOST_CHECK(res); res = solver2.shapeDistance(s1, transform * Transform3f(Vec3f(29.9, 0, 0)), s2, transform, &dist); BOOST_CHECK(dist < 0); BOOST_CHECK_FALSE(res); } template void testReversibleShapeIntersection(const S1& s1, const S2& s2, FCL_REAL distance) { Transform3f tf1(Vec3f(-0.5 * distance, 0.0, 0.0)); Transform3f tf2(Vec3f(+0.5 * distance, 0.0, 0.0)); std::vector contactsA; std::vector contactsB; bool resA; bool resB; const double tol = 1e-6; resA = solver1.shapeIntersect(s1, tf1, s2, tf2, &contactsA); resB = solver1.shapeIntersect(s2, tf2, s1, tf1, &contactsB); // normal should be opposite for (size_t i = 0; i < contactsB.size(); ++i) contactsB[i].normal = -contactsB[i].normal; BOOST_CHECK(resA); BOOST_CHECK(resB); BOOST_CHECK(inspectContactPoints(s1, tf1, s2, tf2, GST_LIBCCD, contactsA, contactsB, true, true, true, false, tol)); resA = solver2.shapeIntersect(s1, tf1, s2, tf2, &contactsA); resB = solver2.shapeIntersect(s2, tf2, s1, tf1, &contactsB); // normal should be opposite for (size_t i = 0; i < contactsB.size(); ++i) contactsB[i].normal = -contactsB[i].normal; BOOST_CHECK(resA); BOOST_CHECK(resB); BOOST_CHECK(inspectContactPoints(s1, tf1, s2, tf2, GST_INDEP, contactsA, contactsB, true, true, true, false, tol)); } BOOST_AUTO_TEST_CASE(reversibleShapeIntersection_allshapes) { // This test check whether a shape intersection algorithm is called for the // reverse case as well. For example, if FCL has sphere-capsule intersection // algorithm, then this algorithm should be called for capsule-sphere case. // Prepare all kinds of primitive shapes (8) -- box, sphere, ellipsoid, capsule, cone, cylinder, plane, halfspace Box box(10, 10, 10); Sphere sphere(5); Ellipsoid ellipsoid(5, 5, 5); Capsule capsule(5, 10); Cone cone(5, 10); Cylinder cylinder(5, 10); Plane plane(Vec3f(), 0.0); Halfspace halfspace(Vec3f(), 0.0); // Use sufficiently short distance so that all the primitive shapes can intersect FCL_REAL distance = 5.0; // If new shape intersection algorithm is added for two distinct primitive // shapes, uncomment associated lines. For example, box-sphere intersection // algorithm is added, then uncomment box-sphere. // testReversibleShapeIntersection(box, sphere, distance); // testReversibleShapeIntersection(box, ellipsoid, distance); // testReversibleShapeIntersection(box, capsule, distance); // testReversibleShapeIntersection(box, cone, distance); // testReversibleShapeIntersection(box, cylinder, distance); testReversibleShapeIntersection(box, plane, distance); testReversibleShapeIntersection(box, halfspace, distance); // testReversibleShapeIntersection(sphere, ellipsoid, distance); testReversibleShapeIntersection(sphere, capsule, distance); // testReversibleShapeIntersection(sphere, cone, distance); // testReversibleShapeIntersection(sphere, cylinder, distance); testReversibleShapeIntersection(sphere, plane, distance); testReversibleShapeIntersection(sphere, halfspace, distance); // testReversibleShapeIntersection(ellipsoid, capsule, distance); // testReversibleShapeIntersection(ellipsoid, cone, distance); // testReversibleShapeIntersection(ellipsoid, cylinder, distance); // testReversibleShapeIntersection(ellipsoid, plane, distance); // testReversibleShapeIntersection(ellipsoid, halfspace, distance); // testReversibleShapeIntersection(capsule, cone, distance); // testReversibleShapeIntersection(capsule, cylinder, distance); testReversibleShapeIntersection(capsule, plane, distance); testReversibleShapeIntersection(capsule, halfspace, distance); // testReversibleShapeIntersection(cone, cylinder, distance); testReversibleShapeIntersection(cone, plane, distance); testReversibleShapeIntersection(cone, halfspace, distance); testReversibleShapeIntersection(cylinder, plane, distance); testReversibleShapeIntersection(cylinder, halfspace, distance); testReversibleShapeIntersection(plane, halfspace, distance); } template void testReversibleShapeDistance(const S1& s1, const S2& s2, FCL_REAL distance) { Transform3f tf1(Vec3f(-0.5 * distance, 0.0, 0.0)); Transform3f tf2(Vec3f(+0.5 * distance, 0.0, 0.0)); FCL_REAL distA; FCL_REAL distB; Vec3f p1A; Vec3f p1B; Vec3f p2A; Vec3f p2B; bool resA; bool resB; const double tol = 1e-6; resA = solver1.shapeDistance(s1, tf1, s2, tf2, &distA, &p1A, &p2A); resB = solver1.shapeDistance(s2, tf2, s1, tf1, &distB, &p1B, &p2B); BOOST_CHECK(resA); BOOST_CHECK(resB); BOOST_CHECK_CLOSE(distA, distB, tol); // distances should be same BOOST_CHECK(p1A.equal(p2B, tol)); // closest points should in reverse order BOOST_CHECK(p2A.equal(p1B, tol)); resA = solver2.shapeDistance(s1, tf1, s2, tf2, &distA, &p1A, &p2A); resB = solver2.shapeDistance(s2, tf2, s1, tf1, &distB, &p1B, &p2B); BOOST_CHECK(resA); BOOST_CHECK(resB); BOOST_CHECK_CLOSE(distA, distB, tol); BOOST_CHECK(p1A.equal(p2B, tol)); BOOST_CHECK(p2A.equal(p1B, tol)); } BOOST_AUTO_TEST_CASE(reversibleShapeDistance_allshapes) { // This test check whether a shape distance algorithm is called for the // reverse case as well. For example, if FCL has sphere-capsule distance // algorithm, then this algorithm should be called for capsule-sphere case. // Prepare all kinds of primitive shapes (8) -- box, sphere, ellipsoid, capsule, cone, cylinder, plane, halfspace Box box(10, 10, 10); Sphere sphere(5); Ellipsoid ellipsoid(5, 5, 5); Capsule capsule(5, 10); Cone cone(5, 10); Cylinder cylinder(5, 10); Plane plane(Vec3f(), 0.0); Halfspace halfspace(Vec3f(), 0.0); // Use sufficiently long distance so that all the primitive shapes CANNOT intersect FCL_REAL distance = 15.0; // If new shape distance algorithm is added for two distinct primitive // shapes, uncomment associated lines. For example, box-sphere intersection // algorithm is added, then uncomment box-sphere. // testReversibleShapeDistance(box, sphere, distance); // testReversibleShapeDistance(box, ellipsoid, distance); // testReversibleShapeDistance(box, capsule, distance); // testReversibleShapeDistance(box, cone, distance); // testReversibleShapeDistance(box, cylinder, distance); // testReversibleShapeDistance(box, plane, distance); // testReversibleShapeDistance(box, halfspace, distance); // testReversibleShapeDistance(sphere, ellipsoid, distance); testReversibleShapeDistance(sphere, capsule, distance); // testReversibleShapeDistance(sphere, cone, distance); // testReversibleShapeDistance(sphere, cylinder, distance); // testReversibleShapeDistance(sphere, plane, distance); // testReversibleShapeDistance(sphere, halfspace, distance); // testReversibleShapeDistance(ellipsoid, capsule, distance); // testReversibleShapeDistance(ellipsoid, cone, distance); // testReversibleShapeDistance(ellipsoid, cylinder, distance); // testReversibleShapeDistance(ellipsoid, plane, distance); // testReversibleShapeDistance(ellipsoid, halfspace, distance); // testReversibleShapeDistance(capsule, cone, distance); // testReversibleShapeDistance(capsule, cylinder, distance); // testReversibleShapeDistance(capsule, plane, distance); // testReversibleShapeDistance(capsule, halfspace, distance); // testReversibleShapeDistance(cone, cylinder, distance); // testReversibleShapeDistance(cone, plane, distance); // testReversibleShapeDistance(cone, halfspace, distance); // testReversibleShapeDistance(cylinder, plane, distance); // testReversibleShapeDistance(cylinder, halfspace, distance); // testReversibleShapeDistance(plane, halfspace, distance); } fcl-0.5.0/test/test_fcl_math.cpp000066400000000000000000000463531274356571000166040ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * Copyright (c) 2016, Toyota Research Institute * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ #define BOOST_TEST_MODULE "FCL_MATH" #include #if FCL_HAVE_SSE #include "fcl/simd/math_simd_details.h" #endif #include "fcl/math/vec_3f.h" #include "fcl/math/matrix_3f.h" #include "fcl/broadphase/morton.h" #include "fcl/config.h" using namespace fcl; BOOST_AUTO_TEST_CASE(vec_test_basic_vec32) { typedef Vec3fX > Vec3f32; Vec3f32 v1(1.0f, 2.0f, 3.0f); BOOST_CHECK(v1[0] == 1.0f); BOOST_CHECK(v1[1] == 2.0f); BOOST_CHECK(v1[2] == 3.0f); Vec3f32 v2 = v1; Vec3f32 v3(3.3f, 4.3f, 5.3f); v1 += v3; BOOST_CHECK(v1.equal(v2 + v3)); v1 -= v3; BOOST_CHECK(v1.equal(v2)); v1 -= v3; BOOST_CHECK(v1.equal(v2 - v3)); v1 += v3; v1 *= v3; BOOST_CHECK(v1.equal(v2 * v3)); v1 /= v3; BOOST_CHECK(v1.equal(v2)); v1 /= v3; BOOST_CHECK(v1.equal(v2 / v3)); v1 *= v3; v1 *= 2.0f; BOOST_CHECK(v1.equal(v2 * 2.0f)); v1 /= 2.0f; BOOST_CHECK(v1.equal(v2)); v1 /= 2.0f; BOOST_CHECK(v1.equal(v2 / 2.0f)); v1 *= 2.0f; v1 += 2.0f; BOOST_CHECK(v1.equal(v2 + 2.0f)); v1 -= 2.0f; BOOST_CHECK(v1.equal(v2)); v1 -= 2.0f; BOOST_CHECK(v1.equal(v2 - 2.0f)); v1 += 2.0f; BOOST_CHECK((-Vec3f32(1.0f, 2.0f, 3.0f)).equal(Vec3f32(-1.0f, -2.0f, -3.0f))); v1 = Vec3f32(1.0f, 2.0f, 3.0f); v2 = Vec3f32(3.0f, 4.0f, 5.0f); BOOST_CHECK((v1.cross(v2)).equal(Vec3f32(-2.0f, 4.0f, -2.0f))); BOOST_CHECK(std::abs(v1.dot(v2) - 26) < 1e-5); v1 = Vec3f32(3.0f, 4.0f, 5.0f); BOOST_CHECK(std::abs(v1.sqrLength() - 50.0) < 1e-5); BOOST_CHECK(std::abs(v1.length() - sqrt(50.0)) < 1e-5); BOOST_CHECK(normalize(v1).equal(v1 / v1.length())); } BOOST_AUTO_TEST_CASE(vec_test_basic_vec64) { typedef Vec3fX > Vec3f64; Vec3f64 v1(1.0, 2.0, 3.0); BOOST_CHECK(v1[0] == 1.0); BOOST_CHECK(v1[1] == 2.0); BOOST_CHECK(v1[2] == 3.0); Vec3f64 v2 = v1; Vec3f64 v3(3.3, 4.3, 5.3); v1 += v3; BOOST_CHECK(v1.equal(v2 + v3)); v1 -= v3; BOOST_CHECK(v1.equal(v2)); v1 -= v3; BOOST_CHECK(v1.equal(v2 - v3)); v1 += v3; v1 *= v3; BOOST_CHECK(v1.equal(v2 * v3)); v1 /= v3; BOOST_CHECK(v1.equal(v2)); v1 /= v3; BOOST_CHECK(v1.equal(v2 / v3)); v1 *= v3; v1 *= 2.0; BOOST_CHECK(v1.equal(v2 * 2.0)); v1 /= 2.0; BOOST_CHECK(v1.equal(v2)); v1 /= 2.0; BOOST_CHECK(v1.equal(v2 / 2.0)); v1 *= 2.0; v1 += 2.0; BOOST_CHECK(v1.equal(v2 + 2.0)); v1 -= 2.0; BOOST_CHECK(v1.equal(v2)); v1 -= 2.0; BOOST_CHECK(v1.equal(v2 - 2.0)); v1 += 2.0; BOOST_CHECK((-Vec3f64(1.0, 2.0, 3.0)).equal(Vec3f64(-1.0, -2.0, -3.0))); v1 = Vec3f64(1.0, 2.0, 3.0); v2 = Vec3f64(3.0, 4.0, 5.0); BOOST_CHECK((v1.cross(v2)).equal(Vec3f64(-2.0, 4.0, -2.0))); BOOST_CHECK(std::abs(v1.dot(v2) - 26) < 1e-5); v1 = Vec3f64(3.0, 4.0, 5.0); BOOST_CHECK(std::abs(v1.sqrLength() - 50.0) < 1e-5); BOOST_CHECK(std::abs(v1.length() - sqrt(50.0)) < 1e-5); BOOST_CHECK(normalize(v1).equal(v1 / v1.length())); v1 = Vec3f64(1.0, 2.0, 3.0); v2 = Vec3f64(3.0, 4.0, 5.0); BOOST_CHECK((v1.cross(v2)).equal(Vec3f64(-2.0, 4.0, -2.0))); BOOST_CHECK(v1.dot(v2) == 26); } #if FCL_HAVE_SSE BOOST_AUTO_TEST_CASE(vec_test_sse_vec32) { typedef Vec3fX Vec3f32; Vec3f32 v1(1.0f, 2.0f, 3.0f); BOOST_CHECK(v1[0] == 1.0f); BOOST_CHECK(v1[1] == 2.0f); BOOST_CHECK(v1[2] == 3.0f); Vec3f32 v2 = v1; Vec3f32 v3(3.3f, 4.3f, 5.3f); v1 += v3; BOOST_CHECK(v1.equal(v2 + v3)); v1 -= v3; BOOST_CHECK(v1.equal(v2)); v1 -= v3; BOOST_CHECK(v1.equal(v2 - v3)); v1 += v3; v1 *= v3; BOOST_CHECK(v1.equal(v2 * v3)); v1 /= v3; BOOST_CHECK(v1.equal(v2)); v1 /= v3; BOOST_CHECK(v1.equal(v2 / v3)); v1 *= v3; v1 *= 2.0f; BOOST_CHECK(v1.equal(v2 * 2.0f)); v1 /= 2.0f; BOOST_CHECK(v1.equal(v2)); v1 /= 2.0f; BOOST_CHECK(v1.equal(v2 / 2.0f)); v1 *= 2.0f; v1 += 2.0f; BOOST_CHECK(v1.equal(v2 + 2.0f)); v1 -= 2.0f; BOOST_CHECK(v1.equal(v2)); v1 -= 2.0f; BOOST_CHECK(v1.equal(v2 - 2.0f)); v1 += 2.0f; BOOST_CHECK((-Vec3f32(1.0f, 2.0f, 3.0f)).equal(Vec3f32(-1.0f, -2.0f, -3.0f))); v1 = Vec3f32(1.0f, 2.0f, 3.0f); v2 = Vec3f32(3.0f, 4.0f, 5.0f); BOOST_CHECK((v1.cross(v2)).equal(Vec3f32(-2.0f, 4.0f, -2.0f))); BOOST_CHECK(std::abs(v1.dot(v2) - 26) < 1e-5); v1 = Vec3f32(3.0f, 4.0f, 5.0f); BOOST_CHECK(std::abs(v1.sqrLength() - 50) < 1e-5); BOOST_CHECK(std::abs(v1.length() - sqrt(50)) < 1e-5); BOOST_CHECK(normalize(v1).equal(v1 / v1.length())); } BOOST_AUTO_TEST_CASE(vec_test_sse_vec64) { typedef Vec3fX Vec3f64; Vec3f64 v1(1.0, 2.0, 3.0); BOOST_CHECK(v1[0] == 1.0); BOOST_CHECK(v1[1] == 2.0); BOOST_CHECK(v1[2] == 3.0); Vec3f64 v2 = v1; Vec3f64 v3(3.3, 4.3, 5.3); v1 += v3; BOOST_CHECK(v1.equal(v2 + v3)); v1 -= v3; BOOST_CHECK(v1.equal(v2)); v1 -= v3; BOOST_CHECK(v1.equal(v2 - v3)); v1 += v3; v1 *= v3; BOOST_CHECK(v1.equal(v2 * v3)); v1 /= v3; BOOST_CHECK(v1.equal(v2)); v1 /= v3; BOOST_CHECK(v1.equal(v2 / v3)); v1 *= v3; v1 *= 2.0; BOOST_CHECK(v1.equal(v2 * 2.0)); v1 /= 2.0; BOOST_CHECK(v1.equal(v2)); v1 /= 2.0; BOOST_CHECK(v1.equal(v2 / 2.0)); v1 *= 2.0; v1 += 2.0; BOOST_CHECK(v1.equal(v2 + 2.0)); v1 -= 2.0; BOOST_CHECK(v1.equal(v2)); v1 -= 2.0; BOOST_CHECK(v1.equal(v2 - 2.0)); v1 += 2.0; BOOST_CHECK((-Vec3f64(1.0, 2.0, 3.0)).equal(Vec3f64(-1.0, -2.0, -3.0))); v1 = Vec3f64(1.0, 2.0, 3.0); v2 = Vec3f64(3.0, 4.0, 5.0); BOOST_CHECK((v1.cross(v2)).equal(Vec3f64(-2.0, 4.0, -2.0))); BOOST_CHECK(std::abs(v1.dot(v2) - 26) < 1e-5); v1 = Vec3f64(3.0, 4.0, 5.0); BOOST_CHECK(std::abs(v1.sqrLength() - 50) < 1e-5); BOOST_CHECK(std::abs(v1.length() - sqrt(50)) < 1e-5); BOOST_CHECK(normalize(v1).equal(v1 / v1.length())); v1 = Vec3f64(1.0, 2.0, 3.0); v2 = Vec3f64(3.0, 4.0, 5.0); BOOST_CHECK((v1.cross(v2)).equal(Vec3f64(-2.0, 4.0, -2.0))); BOOST_CHECK(v1.dot(v2) == 26); } BOOST_AUTO_TEST_CASE(sse_mat32_consistent) { typedef Vec3fX > Vec3f32; typedef Vec3fX Vec3f32SSE; typedef Matrix3fX > Matrix3f32; typedef Matrix3fX Matrix3f32SSE; Vec3f32 v1(1, 2, 3); Vec3f32SSE v2(1, 2, 3); Matrix3f32 m1(-1, 3, -3, 0, -6, 6, -5, -3, 1); Matrix3f32SSE m2(-1, 3, -3, 0, -6, 6, -5, -3, 1); for(size_t i = 0; i < 3; ++i) for(size_t j = 0; j < 3; ++j) BOOST_CHECK((m1(i, j) - m2(i, j) < 1e-1)); Matrix3f32 m3(transpose(m1)); Matrix3f32SSE m4(transpose(m2)); for(size_t i = 0; i < 3; ++i) for(size_t j = 0; j < 3; ++j) BOOST_CHECK((m3(i, j) - m4(i, j) < 1e-1)); m3 = m1; m3.transpose(); m4 = m2; m4.transpose(); for(size_t i = 0; i < 3; ++i) for(size_t j = 0; j < 3; ++j) BOOST_CHECK((m3(i, j) - m4(i, j) < 1e-1)); m3 = inverse(m1); m4 = inverse(m2); for(size_t i = 0; i < 3; ++i) for(size_t j = 0; j < 3; ++j) BOOST_CHECK((m3(i, j) - m4(i, j) < 1e-1)); m3 = m1; m3.inverse(); m4 = m2; m4.inverse(); for(size_t i = 0; i < 3; ++i) for(size_t j = 0; j < 3; ++j) BOOST_CHECK((m3(i, j) - m4(i, j) < 1e-1)); } BOOST_AUTO_TEST_CASE(vec_test_sse_vec32_consistent) { typedef Vec3fX > Vec3f32; typedef Vec3fX Vec3f32SSE; Vec3f32 v1(3.4f, 4.2f, 10.5f), v2(3.1f, 0.1f, -50.4f); Vec3f32SSE v3(3.4f, 4.2f, 10.5f), v4(3.1f, 0.1f, -50.4f); Vec3f32 v12 = v1 + v2; Vec3f32SSE v34 = v3 + v4; BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = v1 - v2; v34 = v3 - v4; BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = v1 * v2; v34 = v3 * v4; BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = v1 / v2; v34 = v3 / v4; BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); float t = 1234.433f; v12 = v1 + t; v34 = v3 + t; BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = v1 - t; v34 = v3 - t; BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = v1 * t; v34 = v3 * t; BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = v1 / t; v34 = v3 / t; BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = v1; v12 += v2; v34 = v3; v34 += v4; BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = v1; v12 -= v2; v34 = v3; v34 -= v4; BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = v1; v12 *= v2; v34 = v3; v34 *= v4; BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = v1; v12 /= v2; v34 = v3; v34 /= v4; BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = v1; v12 += t; v34 = v3; v34 += t; BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = v1; v12 -= t; v34 = v3; v34 -= t; BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = v1; v12 *= t; v34 = v3; v34 *= t; BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = v1; v12 /= t; v34 = v3; v34 /= t; BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = -v1; v34 = -v3; BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = v1.cross(v2); v34 = v3.cross(v4); BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); BOOST_CHECK(std::abs(v1.dot(v2) - v3.dot(v4)) < 1e-5); v12 = min(v1, v2); v34 = min(v3, v4); BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = max(v1, v2); v34 = max(v3, v4); BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = abs(v2); v34 = abs(v4); BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); Vec3f32 delta1(1e-9f, 1e-9f, 1e-9f); Vec3f32SSE delta2(1e-9f, 1e-9f, 1e-9f); BOOST_CHECK((v1 + delta1).equal(v1)); BOOST_CHECK((v3 + delta2).equal(v3)); BOOST_CHECK(std::abs(v1.length() - v3.length()) < 1e-5); BOOST_CHECK(std::abs(v1.sqrLength() - v3.sqrLength()) < 1e-5); v12 = v1; v12.negate(); v34 = v3; v34.negate(); BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = v1; v12.normalize(); v34 = v3; v34.normalize(); BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = normalize(v1); v34 = normalize(v3); BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); } BOOST_AUTO_TEST_CASE(vec_test_sse_vec64_consistent) { typedef Vec3fX > Vec3f64; typedef Vec3fX Vec3f64SSE; Vec3f64 v1(3.4, 4.2, 10.5), v2(3.1, 0.1, -50.4); Vec3f64SSE v3(3.4, 4.2, 10.5), v4(3.1, 0.1, -50.4); Vec3f64 v12 = v1 + v2; Vec3f64SSE v34 = v3 + v4; BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = v1 - v2; v34 = v3 - v4; BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = v1 * v2; v34 = v3 * v4; BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = v1 / v2; v34 = v3 / v4; BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); double t = 1234.433; v12 = v1 + t; v34 = v3 + t; BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = v1 - t; v34 = v3 - t; BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = v1 * t; v34 = v3 * t; BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = v1 / t; v34 = v3 / t; BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = v1; v12 += v2; v34 = v3; v34 += v4; BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = v1; v12 -= v2; v34 = v3; v34 -= v4; BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = v1; v12 *= v2; v34 = v3; v34 *= v4; BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = v1; v12 /= v2; v34 = v3; v34 /= v4; BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = v1; v12 += t; v34 = v3; v34 += t; BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = v1; v12 -= t; v34 = v3; v34 -= t; BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = v1; v12 *= t; v34 = v3; v34 *= t; BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = v1; v12 /= t; v34 = v3; v34 /= t; BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = -v1; v34 = -v3; BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = v1.cross(v2); v34 = v3.cross(v4); BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); BOOST_CHECK(std::abs(v1.dot(v2) - v3.dot(v4)) < 1e-5); v12 = min(v1, v2); v34 = min(v3, v4); BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = max(v1, v2); v34 = max(v3, v4); BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = abs(v2); v34 = abs(v4); BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); Vec3f64 delta1(1e-15, 1e-15, 1e-15); Vec3f64SSE delta2(1e-15, 1e-15, 1e-15); BOOST_CHECK((v1 + delta1).equal(v1)); BOOST_CHECK((v3 + delta2).equal(v3)); BOOST_CHECK(std::abs(v1.length() - v3.length()) < 1e-5); BOOST_CHECK(std::abs(v1.sqrLength() - v3.sqrLength()) < 1e-5); v12 = v1; v12.negate(); v34 = v3; v34.negate(); BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = v1; v12.normalize(); v34 = v3; v34.normalize(); BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); v12 = normalize(v1); v34 = normalize(v3); BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5); BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5); BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5); } #endif BOOST_AUTO_TEST_CASE(morton) { AABB bbox(Vec3f(0, 0, 0), Vec3f(1000, 1000, 1000)); morton_functor> F1(bbox); morton_functor> F2(bbox); morton_functor F3(bbox); // 60 bits morton_functor F4(bbox); // 30 bits Vec3f p(254, 873, 674); BOOST_CHECK(F1(p).to_ulong() == F4(p)); BOOST_CHECK(F2(p).to_ullong() == F3(p)); } fcl-0.5.0/test/test_fcl_octomap.cpp000066400000000000000000000647021274356571000173130ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #define BOOST_TEST_MODULE "FCL_OCTOMAP" #include #include "fcl/config.h" #include "fcl/octree.h" #include "fcl/traversal/traversal_node_octree.h" #include "fcl/collision.h" #include "fcl/broadphase/broadphase.h" #include "fcl/shape/geometric_shape_to_BVH_model.h" #include "fcl/math/transform.h" #include "test_fcl_utility.h" #include "fcl_resources/config.h" #include using namespace fcl; struct TStruct { std::vector records; double overall_time; TStruct() { overall_time = 0; } void push_back(double t) { records.push_back(t); overall_time += t; } }; /// @brief Generate environment with 3 * n objects: n spheres, n boxes and n cylinders void generateEnvironments(std::vector& env, double env_scale, std::size_t n); /// @brief Generate environment with 3 * n objects: n spheres, n boxes and n cylinders, all in mesh void generateEnvironmentsMesh(std::vector& env, double env_scale, std::size_t n); /// @brief Generate boxes from the octomap void generateBoxesFromOctomap(std::vector& env, OcTree& tree); /// @brief Generate boxes from the octomap void generateBoxesFromOctomapMesh(std::vector& env, OcTree& tree); /// @brief Generate an octree octomap::OcTree* generateOcTree(); /// @brief Octomap collision with an environment with 3 * env_size objects void octomap_collision_test(double env_scale, std::size_t env_size, bool exhaustive, std::size_t num_max_contacts, bool use_mesh, bool use_mesh_octomap); /// @brief Octomap collision with an environment with 3 * env_size objects, compute cost void octomap_cost_test(double env_scale, std::size_t env_size, std::size_t num_max_cost_sources, bool use_mesh, bool use_mesh_octomap); /// @brief Octomap distance with an environment with 3 * env_size objects void octomap_distance_test(double env_scale, std::size_t env_size, bool use_mesh, bool use_mesh_octomap); /// @brief Octomap collision with an environment mesh with 3 * env_size objects, asserting that correct triangle ids /// are returned when performing collision tests void octomap_collision_test_mesh_triangle_id(double env_scale, std::size_t env_size, std::size_t num_max_contacts); template void octomap_collision_test_BVH(std::size_t n, bool exhaustive); template void octomap_distance_test_BVH(std::size_t n); BOOST_AUTO_TEST_CASE(test_octomap_cost) { octomap_cost_test(200, 100, 10, false, false); octomap_cost_test(200, 1000, 10, false, false); } BOOST_AUTO_TEST_CASE(test_octomap_cost_mesh) { octomap_cost_test(200, 100, 10, true, false); octomap_cost_test(200, 1000, 10, true, false); } BOOST_AUTO_TEST_CASE(test_octomap_collision) { octomap_collision_test(200, 100, false, 10, false, false); octomap_collision_test(200, 1000, false, 10, false, false); octomap_collision_test(200, 100, true, 1, false, false); octomap_collision_test(200, 1000, true, 1, false, false); } BOOST_AUTO_TEST_CASE(test_octomap_collision_mesh) { #ifdef FCL_BUILD_TYPE_DEBUG octomap_collision_test(200, 10, false, 10, true, true); octomap_collision_test(200, 100, false, 10, true, true); octomap_collision_test(200, 10, true, 1, true, true); octomap_collision_test(200, 100, true, 1, true, true); #else octomap_collision_test(200, 100, false, 10, true, true); octomap_collision_test(200, 1000, false, 10, true, true); octomap_collision_test(200, 100, true, 1, true, true); octomap_collision_test(200, 1000, true, 1, true, true); #endif } BOOST_AUTO_TEST_CASE(test_octomap_collision_mesh_triangle_id) { octomap_collision_test_mesh_triangle_id(1, 30, 100000); } BOOST_AUTO_TEST_CASE(test_octomap_collision_mesh_octomap_box) { #ifdef FCL_BUILD_TYPE_DEBUG octomap_collision_test(200, 10, false, 10, true, false); octomap_collision_test(200, 100, false, 10, true, false); octomap_collision_test(200, 10, true, 1, true, false); octomap_collision_test(200, 100, true, 1, true, false); #else octomap_collision_test(200, 100, false, 10, true, false); octomap_collision_test(200, 1000, false, 10, true, false); octomap_collision_test(200, 100, true, 1, true, false); octomap_collision_test(200, 1000, true, 1, true, false); #endif } BOOST_AUTO_TEST_CASE(test_octomap_distance) { octomap_distance_test(200, 100, false, false); octomap_distance_test(200, 1000, false, false); } BOOST_AUTO_TEST_CASE(test_octomap_distance_mesh) { #ifdef FCL_BUILD_TYPE_DEBUG octomap_distance_test(200, 5, true, true); octomap_distance_test(200, 50, true, true); #else octomap_distance_test(200, 100, true, true); octomap_distance_test(200, 1000, true, true); #endif } BOOST_AUTO_TEST_CASE(test_octomap_distance_mesh_octomap_box) { #ifdef FCL_BUILD_TYPE_DEBUG octomap_distance_test(200, 10, true, false); octomap_distance_test(200, 100, true, false); #else octomap_distance_test(200, 100, true, false); octomap_distance_test(200, 1000, true, false); #endif } BOOST_AUTO_TEST_CASE(test_octomap_bvh_obb_collision_obb) { octomap_collision_test_BVH(5, false); octomap_collision_test_BVH(5, true); } BOOST_AUTO_TEST_CASE(test_octomap_bvh_rss_d_distance_rss) { octomap_distance_test_BVH(5); } BOOST_AUTO_TEST_CASE(test_octomap_bvh_obb_d_distance_obb) { octomap_distance_test_BVH(5); } BOOST_AUTO_TEST_CASE(test_octomap_bvh_kios_d_distance_kios) { #ifdef FCL_BUILD_TYPE_DEBUG octomap_distance_test_BVH(2); #else octomap_distance_test_BVH(5); #endif } template void octomap_collision_test_BVH(std::size_t n, bool exhaustive) { std::vector p1; std::vector t1; boost::filesystem::path path(TEST_RESOURCES_DIR); loadOBJFile((path / "env.obj").string().c_str(), p1, t1); BVHModel* m1 = new BVHModel(); std::shared_ptr m1_ptr(m1); m1->beginModel(); m1->addSubModel(p1, t1); m1->endModel(); OcTree* tree = new OcTree(std::shared_ptr(generateOcTree())); std::shared_ptr tree_ptr(tree); std::vector transforms; FCL_REAL extents[] = {-10, -10, 10, 10, 10, 10}; generateRandomTransforms(extents, transforms, n); for(std::size_t i = 0; i < n; ++i) { Transform3f tf(transforms[i]); CollisionObject obj1(m1_ptr, tf); CollisionObject obj2(tree_ptr, tf); CollisionData cdata; if(exhaustive) cdata.request.num_max_contacts = 100000; defaultCollisionFunction(&obj1, &obj2, &cdata); std::vector boxes; generateBoxesFromOctomap(boxes, *tree); for(std::size_t j = 0; j < boxes.size(); ++j) boxes[j]->setTransform(tf * boxes[j]->getTransform()); DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager(); manager->registerObjects(boxes); manager->setup(); CollisionData cdata2; if(exhaustive) cdata2.request.num_max_contacts = 100000; manager->collide(&obj1, &cdata2, defaultCollisionFunction); for(std::size_t j = 0; j < boxes.size(); ++j) delete boxes[j]; delete manager; if(exhaustive) { std::cout << cdata.result.numContacts() << " " << cdata2.result.numContacts() << std::endl; BOOST_CHECK(cdata.result.numContacts() == cdata2.result.numContacts()); } else { std::cout << (cdata.result.numContacts() > 0) << " " << (cdata2.result.numContacts() > 0) << std::endl; BOOST_CHECK((cdata.result.numContacts() > 0) == (cdata2.result.numContacts() > 0)); } } } template void octomap_distance_test_BVH(std::size_t n) { std::vector p1; std::vector t1; boost::filesystem::path path(TEST_RESOURCES_DIR); loadOBJFile((path / "env.obj").string().c_str(), p1, t1); BVHModel* m1 = new BVHModel(); std::shared_ptr m1_ptr(m1); m1->beginModel(); m1->addSubModel(p1, t1); m1->endModel(); OcTree* tree = new OcTree(std::shared_ptr(generateOcTree())); std::shared_ptr tree_ptr(tree); std::vector transforms; FCL_REAL extents[] = {-10, -10, 10, 10, 10, 10}; generateRandomTransforms(extents, transforms, n); for(std::size_t i = 0; i < n; ++i) { Transform3f tf(transforms[i]); CollisionObject obj1(m1_ptr, tf); CollisionObject obj2(tree_ptr, tf); DistanceData cdata; FCL_REAL dist1 = std::numeric_limits::max(); defaultDistanceFunction(&obj1, &obj2, &cdata, dist1); std::vector boxes; generateBoxesFromOctomap(boxes, *tree); for(std::size_t j = 0; j < boxes.size(); ++j) boxes[j]->setTransform(tf * boxes[j]->getTransform()); DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager(); manager->registerObjects(boxes); manager->setup(); DistanceData cdata2; manager->distance(&obj1, &cdata2, defaultDistanceFunction); FCL_REAL dist2 = cdata2.result.min_distance; for(std::size_t j = 0; j < boxes.size(); ++j) delete boxes[j]; delete manager; std::cout << dist1 << " " << dist2 << std::endl; BOOST_CHECK(std::abs(dist1 - dist2) < 0.001); } } void octomap_cost_test(double env_scale, std::size_t env_size, std::size_t num_max_cost_sources, bool use_mesh, bool use_mesh_octomap) { std::vector env; if(use_mesh) generateEnvironmentsMesh(env, env_scale, env_size); else generateEnvironments(env, env_scale, env_size); OcTree* tree = new OcTree(std::shared_ptr(generateOcTree())); CollisionObject tree_obj((std::shared_ptr(tree))); DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager(); manager->registerObjects(env); manager->setup(); CollisionData cdata; cdata.request.enable_cost = true; cdata.request.num_max_cost_sources = num_max_cost_sources; TStruct t1; Timer timer1; timer1.start(); manager->octree_as_geometry_collide = false; manager->octree_as_geometry_distance = false; manager->collide(&tree_obj, &cdata, defaultCollisionFunction); timer1.stop(); t1.push_back(timer1.getElapsedTime()); CollisionData cdata3; cdata3.request.enable_cost = true; cdata3.request.num_max_cost_sources = num_max_cost_sources; TStruct t3; Timer timer3; timer3.start(); manager->octree_as_geometry_collide = true; manager->octree_as_geometry_distance = true; manager->collide(&tree_obj, &cdata3, defaultCollisionFunction); timer3.stop(); t3.push_back(timer3.getElapsedTime()); TStruct t2; Timer timer2; timer2.start(); std::vector boxes; if(use_mesh_octomap) generateBoxesFromOctomapMesh(boxes, *tree); else generateBoxesFromOctomap(boxes, *tree); timer2.stop(); t2.push_back(timer2.getElapsedTime()); timer2.start(); DynamicAABBTreeCollisionManager* manager2 = new DynamicAABBTreeCollisionManager(); manager2->registerObjects(boxes); manager2->setup(); timer2.stop(); t2.push_back(timer2.getElapsedTime()); CollisionData cdata2; cdata2.request.enable_cost = true; cdata3.request.num_max_cost_sources = num_max_cost_sources; timer2.start(); manager->collide(manager2, &cdata2, defaultCollisionFunction); timer2.stop(); t2.push_back(timer2.getElapsedTime()); std::cout << cdata.result.numContacts() << " " << cdata3.result.numContacts() << " " << cdata2.result.numContacts() << std::endl; std::cout << cdata.result.numCostSources() << " " << cdata3.result.numCostSources() << " " << cdata2.result.numCostSources() << std::endl; { std::vector cost_sources; cdata.result.getCostSources(cost_sources); for(std::size_t i = 0; i < cost_sources.size(); ++i) { std::cout << cost_sources[i].aabb_min << " " << cost_sources[i].aabb_max << " " << cost_sources[i].cost_density << std::endl; } std::cout << std::endl; cdata3.result.getCostSources(cost_sources); for(std::size_t i = 0; i < cost_sources.size(); ++i) { std::cout << cost_sources[i].aabb_min << " " << cost_sources[i].aabb_max << " " << cost_sources[i].cost_density << std::endl; } std::cout << std::endl; cdata2.result.getCostSources(cost_sources); for(std::size_t i = 0; i < cost_sources.size(); ++i) { std::cout << cost_sources[i].aabb_min << " " << cost_sources[i].aabb_max << " " << cost_sources[i].cost_density << std::endl; } std::cout << std::endl; } if(use_mesh) BOOST_CHECK((cdata.result.numContacts() > 0) >= (cdata2.result.numContacts() > 0)); else BOOST_CHECK(cdata.result.numContacts() >= cdata2.result.numContacts()); delete manager; delete manager2; for(std::size_t i = 0; i < boxes.size(); ++i) delete boxes[i]; std::cout << "collision cost" << std::endl; std::cout << "1) octomap overall time: " << t1.overall_time << std::endl; std::cout << "1') octomap overall time (as geometry): " << t3.overall_time << std::endl; std::cout << "2) boxes overall time: " << t2.overall_time << std::endl; std::cout << " a) to boxes: " << t2.records[0] << std::endl; std::cout << " b) structure init: " << t2.records[1] << std::endl; std::cout << " c) collision: " << t2.records[2] << std::endl; std::cout << "Note: octomap may need more collides when using mesh, because octomap collision uses box primitive inside" << std::endl; } void octomap_collision_test(double env_scale, std::size_t env_size, bool exhaustive, std::size_t num_max_contacts, bool use_mesh, bool use_mesh_octomap) { // srand(1); std::vector env; if(use_mesh) generateEnvironmentsMesh(env, env_scale, env_size); else generateEnvironments(env, env_scale, env_size); OcTree* tree = new OcTree(std::shared_ptr(generateOcTree())); CollisionObject tree_obj((std::shared_ptr(tree))); DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager(); manager->registerObjects(env); manager->setup(); CollisionData cdata; if(exhaustive) cdata.request.num_max_contacts = 100000; else cdata.request.num_max_contacts = num_max_contacts; TStruct t1; Timer timer1; timer1.start(); manager->octree_as_geometry_collide = false; manager->octree_as_geometry_distance = false; manager->collide(&tree_obj, &cdata, defaultCollisionFunction); timer1.stop(); t1.push_back(timer1.getElapsedTime()); CollisionData cdata3; if(exhaustive) cdata3.request.num_max_contacts = 100000; else cdata3.request.num_max_contacts = num_max_contacts; TStruct t3; Timer timer3; timer3.start(); manager->octree_as_geometry_collide = true; manager->octree_as_geometry_distance = true; manager->collide(&tree_obj, &cdata3, defaultCollisionFunction); timer3.stop(); t3.push_back(timer3.getElapsedTime()); TStruct t2; Timer timer2; timer2.start(); std::vector boxes; if(use_mesh_octomap) generateBoxesFromOctomapMesh(boxes, *tree); else generateBoxesFromOctomap(boxes, *tree); timer2.stop(); t2.push_back(timer2.getElapsedTime()); timer2.start(); DynamicAABBTreeCollisionManager* manager2 = new DynamicAABBTreeCollisionManager(); manager2->registerObjects(boxes); manager2->setup(); timer2.stop(); t2.push_back(timer2.getElapsedTime()); CollisionData cdata2; if(exhaustive) cdata2.request.num_max_contacts = 100000; else cdata2.request.num_max_contacts = num_max_contacts; timer2.start(); manager->collide(manager2, &cdata2, defaultCollisionFunction); timer2.stop(); t2.push_back(timer2.getElapsedTime()); std::cout << cdata.result.numContacts() << " " << cdata3.result.numContacts() << " " << cdata2.result.numContacts() << std::endl; if(exhaustive) { if(use_mesh) BOOST_CHECK((cdata.result.numContacts() > 0) >= (cdata2.result.numContacts() > 0)); else BOOST_CHECK(cdata.result.numContacts() == cdata2.result.numContacts()); } else { if(use_mesh) BOOST_CHECK((cdata.result.numContacts() > 0) >= (cdata2.result.numContacts() > 0)); else BOOST_CHECK((cdata.result.numContacts() > 0) >= (cdata2.result.numContacts() > 0)); // because AABB return collision when two boxes contact } delete manager; delete manager2; for(size_t i = 0; i < boxes.size(); ++i) delete boxes[i]; if(exhaustive) std::cout << "exhaustive collision" << std::endl; else std::cout << "non exhaustive collision" << std::endl; std::cout << "1) octomap overall time: " << t1.overall_time << std::endl; std::cout << "1') octomap overall time (as geometry): " << t3.overall_time << std::endl; std::cout << "2) boxes overall time: " << t2.overall_time << std::endl; std::cout << " a) to boxes: " << t2.records[0] << std::endl; std::cout << " b) structure init: " << t2.records[1] << std::endl; std::cout << " c) collision: " << t2.records[2] << std::endl; std::cout << "Note: octomap may need more collides when using mesh, because octomap collision uses box primitive inside" << std::endl; } void octomap_collision_test_mesh_triangle_id(double env_scale, std::size_t env_size, std::size_t num_max_contacts) { std::vector env; generateEnvironmentsMesh(env, env_scale, env_size); OcTree* tree = new OcTree(std::shared_ptr(generateOcTree())); CollisionObject tree_obj((std::shared_ptr(tree))); std::vector boxes; generateBoxesFromOctomap(boxes, *tree); for(std::vector::const_iterator cit = env.begin(); cit != env.end(); ++cit) { fcl::CollisionRequest req(num_max_contacts, true); fcl::CollisionResult cResult; fcl::collide(&tree_obj, *cit, req, cResult); for(std::size_t index=0; index* surface = static_cast*> (contact.o2); BOOST_CHECK(surface->num_tris > contact.b2); } } } void octomap_distance_test(double env_scale, std::size_t env_size, bool use_mesh, bool use_mesh_octomap) { // srand(1); std::vector env; if(use_mesh) generateEnvironmentsMesh(env, env_scale, env_size); else generateEnvironments(env, env_scale, env_size); OcTree* tree = new OcTree(std::shared_ptr(generateOcTree())); CollisionObject tree_obj((std::shared_ptr(tree))); DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager(); manager->registerObjects(env); manager->setup(); DistanceData cdata; TStruct t1; Timer timer1; timer1.start(); manager->octree_as_geometry_collide = false; manager->octree_as_geometry_distance = false; manager->distance(&tree_obj, &cdata, defaultDistanceFunction); timer1.stop(); t1.push_back(timer1.getElapsedTime()); DistanceData cdata3; TStruct t3; Timer timer3; timer3.start(); manager->octree_as_geometry_collide = true; manager->octree_as_geometry_distance = true; manager->distance(&tree_obj, &cdata3, defaultDistanceFunction); timer3.stop(); t3.push_back(timer3.getElapsedTime()); TStruct t2; Timer timer2; timer2.start(); std::vector boxes; if(use_mesh_octomap) generateBoxesFromOctomapMesh(boxes, *tree); else generateBoxesFromOctomap(boxes, *tree); timer2.stop(); t2.push_back(timer2.getElapsedTime()); timer2.start(); DynamicAABBTreeCollisionManager* manager2 = new DynamicAABBTreeCollisionManager(); manager2->registerObjects(boxes); manager2->setup(); timer2.stop(); t2.push_back(timer2.getElapsedTime()); DistanceData cdata2; timer2.start(); manager->distance(manager2, &cdata2, defaultDistanceFunction); timer2.stop(); t2.push_back(timer2.getElapsedTime()); std::cout << cdata.result.min_distance << " " << cdata3.result.min_distance << " " << cdata2.result.min_distance << std::endl; if(cdata.result.min_distance < 0) BOOST_CHECK(cdata2.result.min_distance <= 0); else BOOST_CHECK(std::abs(cdata.result.min_distance - cdata2.result.min_distance) < 1e-3); delete manager; delete manager2; for(size_t i = 0; i < boxes.size(); ++i) delete boxes[i]; std::cout << "1) octomap overall time: " << t1.overall_time << std::endl; std::cout << "1') octomap overall time (as geometry): " << t3.overall_time << std::endl; std::cout << "2) boxes overall time: " << t2.overall_time << std::endl; std::cout << " a) to boxes: " << t2.records[0] << std::endl; std::cout << " b) structure init: " << t2.records[1] << std::endl; std::cout << " c) distance: " << t2.records[2] << std::endl; std::cout << "Note: octomap may need more collides when using mesh, because octomap collision uses box primitive inside" << std::endl; } void generateBoxesFromOctomap(std::vector& boxes, OcTree& tree) { std::vector > boxes_ = tree.toBoxes(); for(std::size_t i = 0; i < boxes_.size(); ++i) { FCL_REAL x = boxes_[i][0]; FCL_REAL y = boxes_[i][1]; FCL_REAL z = boxes_[i][2]; FCL_REAL size = boxes_[i][3]; FCL_REAL cost = boxes_[i][4]; FCL_REAL threshold = boxes_[i][5]; Box* box = new Box(size, size, size); box->cost_density = cost; box->threshold_occupied = threshold; CollisionObject* obj = new CollisionObject(std::shared_ptr(box), Transform3f(Vec3f(x, y, z))); boxes.push_back(obj); } std::cout << "boxes size: " << boxes.size() << std::endl; } void generateEnvironments(std::vector& env, double env_scale, std::size_t n) { FCL_REAL extents[] = {-env_scale, env_scale, -env_scale, env_scale, -env_scale, env_scale}; std::vector transforms; generateRandomTransforms(extents, transforms, n); for(std::size_t i = 0; i < n; ++i) { Box* box = new Box(5, 10, 20); env.push_back(new CollisionObject(std::shared_ptr(box), transforms[i])); } generateRandomTransforms(extents, transforms, n); for(std::size_t i = 0; i < n; ++i) { Sphere* sphere = new Sphere(30); env.push_back(new CollisionObject(std::shared_ptr(sphere), transforms[i])); } generateRandomTransforms(extents, transforms, n); for(std::size_t i = 0; i < n; ++i) { Cylinder* cylinder = new Cylinder(10, 40); env.push_back(new CollisionObject(std::shared_ptr(cylinder), transforms[i])); } } void generateBoxesFromOctomapMesh(std::vector& boxes, OcTree& tree) { std::vector > boxes_ = tree.toBoxes(); for(std::size_t i = 0; i < boxes_.size(); ++i) { FCL_REAL x = boxes_[i][0]; FCL_REAL y = boxes_[i][1]; FCL_REAL z = boxes_[i][2]; FCL_REAL size = boxes_[i][3]; FCL_REAL cost = boxes_[i][4]; FCL_REAL threshold = boxes_[i][5]; Box box(size, size, size); BVHModel* model = new BVHModel(); generateBVHModel(*model, box, Transform3f()); model->cost_density = cost; model->threshold_occupied = threshold; CollisionObject* obj = new CollisionObject(std::shared_ptr(model), Transform3f(Vec3f(x, y, z))); boxes.push_back(obj); } std::cout << "boxes size: " << boxes.size() << std::endl; } void generateEnvironmentsMesh(std::vector& env, double env_scale, std::size_t n) { FCL_REAL extents[] = {-env_scale, env_scale, -env_scale, env_scale, -env_scale, env_scale}; std::vector transforms; generateRandomTransforms(extents, transforms, n); Box box(5, 10, 20); for(std::size_t i = 0; i < n; ++i) { BVHModel* model = new BVHModel(); generateBVHModel(*model, box, Transform3f()); env.push_back(new CollisionObject(std::shared_ptr(model), transforms[i])); } generateRandomTransforms(extents, transforms, n); Sphere sphere(30); for(std::size_t i = 0; i < n; ++i) { BVHModel* model = new BVHModel(); generateBVHModel(*model, sphere, Transform3f(), 16, 16); env.push_back(new CollisionObject(std::shared_ptr(model), transforms[i])); } generateRandomTransforms(extents, transforms, n); Cylinder cylinder(10, 40); for(std::size_t i = 0; i < n; ++i) { BVHModel* model = new BVHModel(); generateBVHModel(*model, cylinder, Transform3f(), 16, 16); env.push_back(new CollisionObject(std::shared_ptr(model), transforms[i])); } } octomap::OcTree* generateOcTree() { octomap::OcTree* tree = new octomap::OcTree(0.1); // insert some measurements of occupied cells for(int x = -20; x < 20; x++) { for(int y = -20; y < 20; y++) { for(int z = -20; z < 20; z++) { tree->updateNode(octomap::point3d(x * 0.05, y * 0.05, z * 0.05), true); } } } // insert some measurements of free cells for(int x = -30; x < 30; x++) { for(int y = -30; y < 30; y++) { for(int z = -30; z < 30; z++) { tree->updateNode(octomap::point3d(x*0.02 -1.0, y*0.02-1.0, z*0.02-1.0), false); } } } return tree; } fcl-0.5.0/test/test_fcl_shape_mesh_consistency.cpp000066400000000000000000003000441274356571000223760ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #define BOOST_TEST_MODULE "FCL_SHAPE_MESH_CONSISTENCY" #include #include "fcl/narrowphase/narrowphase.h" #include "fcl/shape/geometric_shape_to_BVH_model.h" #include "fcl/distance.h" #include "fcl/collision.h" #include "test_fcl_utility.h" using namespace fcl; #define BOOST_CHECK_FALSE(p) BOOST_CHECK(!(p)) FCL_REAL extents[6] = {0, 0, 0, 10, 10, 10}; BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere_libccd) { Sphere s1(20); Sphere s2(20); BVHModel s1_rss; BVHModel s2_rss; generateBVHModel(s1_rss, s1, Transform3f(), 16, 16); generateBVHModel(s2_rss, s2, Transform3f(), 16, 16); DistanceRequest request; DistanceResult res, res1; Transform3f pose; pose.setTranslation(Vec3f(50, 0, 0)); res.clear(); res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1_rss, Transform3f(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); for(std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); Transform3f pose1(t); Transform3f pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); } pose.setTranslation(Vec3f(40.1, 0, 0)); res.clear(), res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, Transform3f(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); for(std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); Transform3f pose1(t); Transform3f pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); } } BOOST_AUTO_TEST_CASE(consistency_distance_ellipsoidellipsoid_libccd) { Ellipsoid s1(20, 40, 50); Ellipsoid s2(10, 10, 10); BVHModel s1_rss; BVHModel s2_rss; generateBVHModel(s1_rss, s1, Transform3f(), 16, 16); generateBVHModel(s2_rss, s2, Transform3f(), 16, 16); DistanceRequest request; DistanceResult res, res1; Transform3f pose; pose.setTranslation(Vec3f(40, 0, 0)); res.clear(); res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1_rss, Transform3f(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); for(std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); Transform3f pose1(t); Transform3f pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); } pose.setTranslation(Vec3f(30.1, 0, 0)); res.clear(), res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, Transform3f(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); for(std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); Transform3f pose1(t); Transform3f pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); } } BOOST_AUTO_TEST_CASE(consistency_distance_boxbox_libccd) { Box s1(20, 40, 50); Box s2(10, 10, 10); BVHModel s1_rss; BVHModel s2_rss; generateBVHModel(s1_rss, s1, Transform3f()); generateBVHModel(s2_rss, s2, Transform3f()); DistanceRequest request; DistanceResult res, res1; Transform3f pose; pose.setTranslation(Vec3f(50, 0, 0)); res.clear(); res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); distance(&s1_rss, Transform3f(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); for(std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); Transform3f pose1(t); Transform3f pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); } pose.setTranslation(Vec3f(15.1, 0, 0)); res.clear(); res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, Transform3f(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); for(std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); Transform3f pose1(t); Transform3f pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); } } BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder_libccd) { Cylinder s1(5, 10); Cylinder s2(5, 10); BVHModel s1_rss; BVHModel s2_rss; generateBVHModel(s1_rss, s1, Transform3f(), 16, 16); generateBVHModel(s2_rss, s2, Transform3f(), 16, 16); DistanceRequest request; DistanceResult res, res1; Transform3f pose; pose.setTranslation(Vec3f(20, 0, 0)); res.clear(); res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); distance(&s1_rss, Transform3f(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); for(std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); Transform3f pose1(t); Transform3f pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); } pose.setTranslation(Vec3f(15, 0, 0)); // libccd cannot use small value here :( res.clear(); res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, Transform3f(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); for(std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); Transform3f pose1(t); Transform3f pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); } } BOOST_AUTO_TEST_CASE(consistency_distance_conecone_libccd) { Cone s1(5, 10); Cone s2(5, 10); BVHModel s1_rss; BVHModel s2_rss; generateBVHModel(s1_rss, s1, Transform3f(), 16, 16); generateBVHModel(s2_rss, s2, Transform3f(), 16, 16); DistanceRequest request; DistanceResult res, res1; Transform3f pose; pose.setTranslation(Vec3f(20, 0, 0)); res.clear(); res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); distance(&s1_rss, Transform3f(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); for(std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); Transform3f pose1(t); Transform3f pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); } pose.setTranslation(Vec3f(15, 0, 0)); // libccd cannot use small value here :( res.clear(); res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, Transform3f(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); for(std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); Transform3f pose1(t); Transform3f pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); } } BOOST_AUTO_TEST_CASE(consistency_distance_spheresphere_GJK) { Sphere s1(20); Sphere s2(20); BVHModel s1_rss; BVHModel s2_rss; generateBVHModel(s1_rss, s1, Transform3f(), 16, 16); generateBVHModel(s2_rss, s2, Transform3f(), 16, 16); DistanceRequest request; request.gjk_solver_type = GST_INDEP; DistanceResult res, res1; Transform3f pose; pose.setTranslation(Vec3f(50, 0, 0)); res.clear(); res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1_rss, Transform3f(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); for(std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); Transform3f pose1(t); Transform3f pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); } pose.setTranslation(Vec3f(40.1, 0, 0)); res.clear(); res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, Transform3f(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 4); for(std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); Transform3f pose1(t); Transform3f pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 4); } } BOOST_AUTO_TEST_CASE(consistency_distance_ellipsoidellipsoid_GJK) { Ellipsoid s1(20, 40, 50); Ellipsoid s2(10, 10, 10); BVHModel s1_rss; BVHModel s2_rss; generateBVHModel(s1_rss, s1, Transform3f(), 16, 16); generateBVHModel(s2_rss, s2, Transform3f(), 16, 16); DistanceRequest request; request.gjk_solver_type = GST_INDEP; DistanceResult res, res1; Transform3f pose; pose.setTranslation(Vec3f(40, 0, 0)); res.clear(); res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1_rss, Transform3f(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); for(std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); Transform3f pose1(t); Transform3f pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); } pose.setTranslation(Vec3f(30.1, 0, 0)); res.clear(); res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, Transform3f(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 4); for(std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); Transform3f pose1(t); Transform3f pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 4); } } BOOST_AUTO_TEST_CASE(consistency_distance_boxbox_GJK) { Box s1(20, 40, 50); Box s2(10, 10, 10); BVHModel s1_rss; BVHModel s2_rss; generateBVHModel(s1_rss, s1, Transform3f()); generateBVHModel(s2_rss, s2, Transform3f()); DistanceRequest request; request.gjk_solver_type = GST_INDEP; DistanceResult res, res1; Transform3f pose; pose.setTranslation(Vec3f(50, 0, 0)); res.clear(); res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); distance(&s1_rss, Transform3f(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); for(std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); Transform3f pose1(t); Transform3f pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.01); } pose.setTranslation(Vec3f(15.1, 0, 0)); res.clear(); res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, Transform3f(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); for(std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); Transform3f pose1(t); Transform3f pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); } } BOOST_AUTO_TEST_CASE(consistency_distance_cylindercylinder_GJK) { Cylinder s1(5, 10); Cylinder s2(5, 10); BVHModel s1_rss; BVHModel s2_rss; generateBVHModel(s1_rss, s1, Transform3f(), 16, 16); generateBVHModel(s2_rss, s2, Transform3f(), 16, 16); DistanceRequest request; request.gjk_solver_type = GST_INDEP; DistanceResult res, res1; Transform3f pose; pose.setTranslation(Vec3f(20, 0, 0)); res.clear(); res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1_rss, Transform3f(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); for(std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); Transform3f pose1(t); Transform3f pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); if(fabs(res1.min_distance - res.min_distance) / res.min_distance > 0.05) std::cout << "low resolution: " << res1.min_distance << " " << res.min_distance << std::endl; else BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); if(fabs(res1.min_distance - res.min_distance) / res.min_distance > 0.05) std::cout << "low resolution: " << res1.min_distance << " " << res.min_distance << std::endl; else BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); if(fabs(res1.min_distance - res.min_distance) / res.min_distance > 0.05) std::cout << "low resolution: " << res1.min_distance << " " << res.min_distance << std::endl; else BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); } pose.setTranslation(Vec3f(10.1, 0, 0)); res.clear(); res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, Transform3f(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); for(std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); Transform3f pose1(t); Transform3f pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); } } BOOST_AUTO_TEST_CASE(consistency_distance_conecone_GJK) { Cone s1(5, 10); Cone s2(5, 10); BVHModel s1_rss; BVHModel s2_rss; generateBVHModel(s1_rss, s1, Transform3f(), 16, 16); generateBVHModel(s2_rss, s2, Transform3f(), 16, 16); DistanceRequest request; request.gjk_solver_type = GST_INDEP; DistanceResult res, res1; Transform3f pose; pose.setTranslation(Vec3f(20, 0, 0)); res.clear(); res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1_rss, Transform3f(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); for(std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); Transform3f pose1(t); Transform3f pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 0.05); } pose.setTranslation(Vec3f(10.1, 0, 0)); res.clear(); res1.clear(); distance(&s1, Transform3f(), &s2, pose, request, res); distance(&s1_rss, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, Transform3f(), &s2_rss, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, Transform3f(), &s2, pose, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); for(std::size_t i = 0; i < 10; ++i) { Transform3f t; generateRandomTransform(extents, t); Transform3f pose1(t); Transform3f pose2 = t * pose; res.clear(); res1.clear(); distance(&s1, pose1, &s2, pose2, request, res); distance(&s1_rss, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1, pose1, &s2_rss, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); res1.clear(); distance(&s1_rss, pose1, &s2, pose2, request, res1); BOOST_CHECK(fabs(res1.min_distance - res.min_distance) / res.min_distance < 2); } } BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_libccd) { Sphere s1(20); Sphere s2(10); BVHModel s1_aabb; BVHModel s2_aabb; BVHModel s1_obb; BVHModel s2_obb; generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16); generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16); generateBVHModel(s1_obb, s1, Transform3f(), 16, 16); generateBVHModel(s2_obb, s2, Transform3f(), 16, 16); CollisionRequest request; CollisionResult result; bool res; Transform3f pose, pose_aabb, pose_obb; // s2 is within s1 // both are shapes --> collision // s1 is shape, s2 is mesh --> in collision // s1 is mesh, s2 is shape --> collision free // all are reasonable result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3f(40, 0, 0)); pose_aabb.setTranslation(Vec3f(40, 0, 0)); pose_obb.setTranslation(Vec3f(40, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3f(30, 0, 0)); pose_aabb.setTranslation(Vec3f(30, 0, 0)); pose_obb.setTranslation(Vec3f(30, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3f(29.9, 0, 0)); pose_aabb.setTranslation(Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision pose_obb.setTranslation(Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3f(-29.9, 0, 0)); pose_aabb.setTranslation(Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision pose_obb.setTranslation(Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3f(-30, 0, 0)); pose_aabb.setTranslation(Vec3f(-30, 0, 0)); pose_obb.setTranslation(Vec3f(-30, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); } BOOST_AUTO_TEST_CASE(consistency_collision_ellipsoidellipsoid_libccd) { Ellipsoid s1(20, 40, 50); Ellipsoid s2(10, 10, 10); BVHModel s1_aabb; BVHModel s2_aabb; BVHModel s1_obb; BVHModel s2_obb; generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16); generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16); generateBVHModel(s1_obb, s1, Transform3f(), 16, 16); generateBVHModel(s2_obb, s2, Transform3f(), 16, 16); CollisionRequest request; CollisionResult result; bool res; Transform3f pose, pose_aabb, pose_obb; // s2 is within s1 // both are shapes --> collision // s1 is shape, s2 is mesh --> in collision // s1 is mesh, s2 is shape --> collision free // all are reasonable result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3f(40, 0, 0)); pose_aabb.setTranslation(Vec3f(40, 0, 0)); pose_obb.setTranslation(Vec3f(40, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3f(30, 0, 0)); pose_aabb.setTranslation(Vec3f(30, 0, 0)); pose_obb.setTranslation(Vec3f(30, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); // libccd cannot detect collision when two ellipsoid is exactly touching each other result.clear(); res = (collide(&s1, Transform3f(), &s2, Transform3f(Vec3f(29.999, 0, 0)), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3f(29.9, 0, 0)); pose_aabb.setTranslation(Vec3f(29.9, 0, 0)); // 29.9 fails, result depends on mesh precision pose_obb.setTranslation(Vec3f(29.9, 0, 0)); // 29.9 fails, result depends on mesh precision result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3f(-29.9, 0, 0)); pose_aabb.setTranslation(Vec3f(-29.9, 0, 0)); pose_obb.setTranslation(Vec3f(-29.9, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3f(-30, 0, 0)); pose_aabb.setTranslation(Vec3f(-30, 0, 0)); pose_obb.setTranslation(Vec3f(-30, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); } BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_libccd) { Box s1(20, 40, 50); Box s2(10, 10, 10); BVHModel s1_aabb; BVHModel s2_aabb; BVHModel s1_obb; BVHModel s2_obb; generateBVHModel(s1_aabb, s1, Transform3f()); generateBVHModel(s2_aabb, s2, Transform3f()); generateBVHModel(s1_obb, s1, Transform3f()); generateBVHModel(s2_obb, s2, Transform3f()); CollisionRequest request; CollisionResult result; bool res; Transform3f pose, pose_aabb, pose_obb; // s2 is within s1 // both are shapes --> collision // s1 is shape, s2 is mesh --> in collision // s1 is mesh, s2 is shape --> collision free // all are reasonable result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3f(15.01, 0, 0)); pose_aabb.setTranslation(Vec3f(15.01, 0, 0)); pose_obb.setTranslation(Vec3f(15.01, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3f(14.99, 0, 0)); pose_aabb.setTranslation(Vec3f(14.99, 0, 0)); pose_obb.setTranslation(Vec3f(14.99, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); } BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_libccd) { Sphere s1(20); Box s2(5, 5, 5); BVHModel s1_aabb; BVHModel s2_aabb; BVHModel s1_obb; BVHModel s2_obb; generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16); generateBVHModel(s2_aabb, s2, Transform3f()); generateBVHModel(s1_obb, s1, Transform3f(), 16, 16); generateBVHModel(s2_obb, s2, Transform3f()); CollisionRequest request; CollisionResult result; bool res; Transform3f pose, pose_aabb, pose_obb; // s2 is within s1 // both are shapes --> collision // s1 is shape, s2 is mesh --> in collision // s1 is mesh, s2 is shape --> collision free // all are reasonable result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3f(22.4, 0, 0)); pose_aabb.setTranslation(Vec3f(22.4, 0, 0)); pose_obb.setTranslation(Vec3f(22.4, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3f(22.51, 0, 0)); pose_aabb.setTranslation(Vec3f(22.51, 0, 0)); pose_obb.setTranslation(Vec3f(22.51, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); } BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder_libccd) { Cylinder s1(5, 10); Cylinder s2(5, 10); BVHModel s1_aabb; BVHModel s2_aabb; BVHModel s1_obb; BVHModel s2_obb; generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16); generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16); generateBVHModel(s1_obb, s1, Transform3f(), 16, 16); generateBVHModel(s2_obb, s2, Transform3f(), 16, 16); CollisionRequest request; CollisionResult result; bool res; Transform3f pose, pose_aabb, pose_obb; pose.setTranslation(Vec3f(9.99, 0, 0)); pose_aabb.setTranslation(Vec3f(9.99, 0, 0)); pose_obb.setTranslation(Vec3f(9.99, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3f(10.01, 0, 0)); pose_aabb.setTranslation(Vec3f(10.01, 0, 0)); pose_obb.setTranslation(Vec3f(10.01, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); } BOOST_AUTO_TEST_CASE(consistency_collision_conecone_libccd) { Cone s1(5, 10); Cone s2(5, 10); BVHModel s1_aabb; BVHModel s2_aabb; BVHModel s1_obb; BVHModel s2_obb; generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16); generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16); generateBVHModel(s1_obb, s1, Transform3f(), 16, 16); generateBVHModel(s2_obb, s2, Transform3f(), 16, 16); CollisionRequest request; CollisionResult result; bool res; Transform3f pose, pose_aabb, pose_obb; pose.setTranslation(Vec3f(9.9, 0, 0)); pose_aabb.setTranslation(Vec3f(9.9, 0, 0)); pose_obb.setTranslation(Vec3f(9.9, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3f(10.1, 0, 0)); pose_aabb.setTranslation(Vec3f(10.1, 0, 0)); pose_obb.setTranslation(Vec3f(10.1, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3f(0, 0, 9.9)); pose_aabb.setTranslation(Vec3f(0, 0, 9.9)); pose_obb.setTranslation(Vec3f(0, 0, 9.9)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3f(0, 0, 10.1)); pose_aabb.setTranslation(Vec3f(0, 0, 10.1)); pose_obb.setTranslation(Vec3f(0, 0, 10.1)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); } BOOST_AUTO_TEST_CASE(consistency_collision_spheresphere_GJK) { Sphere s1(20); Sphere s2(10); BVHModel s1_aabb; BVHModel s2_aabb; BVHModel s1_obb; BVHModel s2_obb; generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16); generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16); generateBVHModel(s1_obb, s1, Transform3f(), 16, 16); generateBVHModel(s2_obb, s2, Transform3f(), 16, 16); CollisionRequest request; request.gjk_solver_type = GST_INDEP; CollisionResult result; bool res; Transform3f pose, pose_aabb, pose_obb; // s2 is within s1 // both are shapes --> collision // s1 is shape, s2 is mesh --> in collision // s1 is mesh, s2 is shape --> collision free // all are reasonable result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3f(40, 0, 0)); pose_aabb.setTranslation(Vec3f(40, 0, 0)); pose_obb.setTranslation(Vec3f(40, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3f(30, 0, 0)); pose_aabb.setTranslation(Vec3f(30, 0, 0)); pose_obb.setTranslation(Vec3f(30, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3f(29.9, 0, 0)); pose_aabb.setTranslation(Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision pose_obb.setTranslation(Vec3f(29.8, 0, 0)); // 29.9 fails, result depends on mesh precision result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3f(-29.9, 0, 0)); pose_aabb.setTranslation(Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision pose_obb.setTranslation(Vec3f(-29.8, 0, 0)); // 29.9 fails, result depends on mesh precision result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3f(-30, 0, 0)); pose_aabb.setTranslation(Vec3f(-30, 0, 0)); pose_obb.setTranslation(Vec3f(-30, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); } BOOST_AUTO_TEST_CASE(consistency_collision_ellipsoidellipsoid_GJK) { Ellipsoid s1(20, 40, 50); Ellipsoid s2(10, 10, 10); BVHModel s1_aabb; BVHModel s2_aabb; BVHModel s1_obb; BVHModel s2_obb; generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16); generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16); generateBVHModel(s1_obb, s1, Transform3f(), 16, 16); generateBVHModel(s2_obb, s2, Transform3f(), 16, 16); CollisionRequest request; request.gjk_solver_type = GST_INDEP; CollisionResult result; bool res; Transform3f pose, pose_aabb, pose_obb; // s2 is within s1 // both are shapes --> collision // s1 is shape, s2 is mesh --> in collision // s1 is mesh, s2 is shape --> collision free // all are reasonable result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3f(40, 0, 0)); pose_aabb.setTranslation(Vec3f(40, 0, 0)); pose_obb.setTranslation(Vec3f(40, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3f(30, 0, 0)); pose_aabb.setTranslation(Vec3f(30, 0, 0)); pose_obb.setTranslation(Vec3f(30, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3f(29.9, 0, 0)); pose_aabb.setTranslation(Vec3f(29.9, 0, 0)); pose_obb.setTranslation(Vec3f(29.9, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3f(-29.9, 0, 0)); pose_aabb.setTranslation(Vec3f(-29.9, 0, 0)); pose_obb.setTranslation(Vec3f(-29.9, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3f(-30, 0, 0)); pose_aabb.setTranslation(Vec3f(-30, 0, 0)); pose_obb.setTranslation(Vec3f(-30, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); } BOOST_AUTO_TEST_CASE(consistency_collision_boxbox_GJK) { Box s1(20, 40, 50); Box s2(10, 10, 10); BVHModel s1_aabb; BVHModel s2_aabb; BVHModel s1_obb; BVHModel s2_obb; generateBVHModel(s1_aabb, s1, Transform3f()); generateBVHModel(s2_aabb, s2, Transform3f()); generateBVHModel(s1_obb, s1, Transform3f()); generateBVHModel(s2_obb, s2, Transform3f()); CollisionRequest request; request.gjk_solver_type = GST_INDEP; CollisionResult result; bool res; Transform3f pose, pose_aabb, pose_obb; // s2 is within s1 // both are shapes --> collision // s1 is shape, s2 is mesh --> in collision // s1 is mesh, s2 is shape --> collision free // all are reasonable result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3f(15.01, 0, 0)); pose_aabb.setTranslation(Vec3f(15.01, 0, 0)); pose_obb.setTranslation(Vec3f(15.01, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3f(14.99, 0, 0)); pose_aabb.setTranslation(Vec3f(14.99, 0, 0)); pose_obb.setTranslation(Vec3f(14.99, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); } BOOST_AUTO_TEST_CASE(consistency_collision_spherebox_GJK) { Sphere s1(20); Box s2(5, 5, 5); BVHModel s1_aabb; BVHModel s2_aabb; BVHModel s1_obb; BVHModel s2_obb; generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16); generateBVHModel(s2_aabb, s2, Transform3f()); generateBVHModel(s1_obb, s1, Transform3f(), 16, 16); generateBVHModel(s2_obb, s2, Transform3f()); CollisionRequest request; request.gjk_solver_type = GST_INDEP; CollisionResult result; bool res; Transform3f pose, pose_aabb, pose_obb; // s2 is within s1 // both are shapes --> collision // s1 is shape, s2 is mesh --> in collision // s1 is mesh, s2 is shape --> collision free // all are reasonable result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3f(22.4, 0, 0)); pose_aabb.setTranslation(Vec3f(22.4, 0, 0)); pose_obb.setTranslation(Vec3f(22.4, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3f(22.51, 0, 0)); pose_aabb.setTranslation(Vec3f(22.51, 0, 0)); pose_obb.setTranslation(Vec3f(22.51, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); } BOOST_AUTO_TEST_CASE(consistency_collision_cylindercylinder_GJK) { Cylinder s1(5, 10); Cylinder s2(5, 10); BVHModel s1_aabb; BVHModel s2_aabb; BVHModel s1_obb; BVHModel s2_obb; generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16); generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16); generateBVHModel(s1_obb, s1, Transform3f(), 16, 16); generateBVHModel(s2_obb, s2, Transform3f(), 16, 16); CollisionRequest request; request.gjk_solver_type = GST_INDEP; CollisionResult result; bool res; Transform3f pose, pose_aabb, pose_obb; pose.setTranslation(Vec3f(9.99, 0, 0)); pose_aabb.setTranslation(Vec3f(9.99, 0, 0)); pose_obb.setTranslation(Vec3f(9.99, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3f(10.01, 0, 0)); pose_aabb.setTranslation(Vec3f(10.01, 0, 0)); pose_obb.setTranslation(Vec3f(10.01, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); } BOOST_AUTO_TEST_CASE(consistency_collision_conecone_GJK) { Cone s1(5, 10); Cone s2(5, 10); BVHModel s1_aabb; BVHModel s2_aabb; BVHModel s1_obb; BVHModel s2_obb; generateBVHModel(s1_aabb, s1, Transform3f(), 16, 16); generateBVHModel(s2_aabb, s2, Transform3f(), 16, 16); generateBVHModel(s1_obb, s1, Transform3f(), 16, 16); generateBVHModel(s2_obb, s2, Transform3f(), 16, 16); CollisionRequest request; request.gjk_solver_type = GST_INDEP; CollisionResult result; bool res; Transform3f pose, pose_aabb, pose_obb; pose.setTranslation(Vec3f(9.9, 0, 0)); pose_aabb.setTranslation(Vec3f(9.9, 0, 0)); pose_obb.setTranslation(Vec3f(9.9, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3f(10.1, 0, 0)); pose_aabb.setTranslation(Vec3f(10.1, 0, 0)); pose_obb.setTranslation(Vec3f(10.1, 0, 0)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); pose.setTranslation(Vec3f(0, 0, 9.9)); pose_aabb.setTranslation(Vec3f(0, 0, 9.9)); pose_obb.setTranslation(Vec3f(0, 0, 9.9)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK(res); pose.setTranslation(Vec3f(0, 0, 10.1)); pose_aabb.setTranslation(Vec3f(0, 0, 10.1)); pose_obb.setTranslation(Vec3f(0, 0, 10.1)); result.clear(); res = (collide(&s1, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_aabb, pose_aabb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2_obb, pose_obb, &s1, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_aabb, pose_aabb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1, Transform3f(), &s2_obb, pose_obb, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_aabb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s2, pose, &s1_obb, Transform3f(), request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); result.clear(); res = (collide(&s1_aabb, Transform3f(), &s2, pose, request, result) > 0); BOOST_CHECK_FALSE(res); } fcl-0.5.0/test/test_fcl_simple.cpp000066400000000000000000000255321274356571000171400ustar00rootroot00000000000000#define BOOST_TEST_MODULE "FCL_SIMPLE" #include #include "fcl/intersect.h" #include "fcl/collision.h" #include "fcl/BVH/BVH_model.h" #include "fcl_resources/config.h" #include #include #include "fcl/math/vec_nf.h" #include "fcl/math/sampling.h" using namespace fcl; static FCL_REAL epsilon = 1e-6; static bool approx(FCL_REAL x, FCL_REAL y) { return std::abs(x - y) < epsilon; } template double distance_Vecnf(const Vecnf& a, const Vecnf& b) { double d = 0; for(std::size_t i = 0; i < N; ++i) d += (a[i] - b[i]) * (a[i] - b[i]); return d; } BOOST_AUTO_TEST_CASE(Vec_nf_test) { Vecnf<4> a; Vecnf<4> b; for(std::size_t i = 0; i < a.dim(); ++i) a[i] = i; for(std::size_t i = 0; i < b.dim(); ++i) b[i] = 1; std::cout << a << std::endl; std::cout << b << std::endl; std::cout << a + b << std::endl; std::cout << a - b << std::endl; std::cout << (a -= b) << std::endl; std::cout << (a += b) << std::endl; std::cout << a * 2 << std::endl; std::cout << a / 2 << std::endl; std::cout << (a *= 2) << std::endl; std::cout << (a /= 2) << std::endl; std::cout << a.dot(b) << std::endl; Vecnf<8> c = combine(a, b); std::cout << c << std::endl; Vecnf<4> upper, lower; for(int i = 0; i < 4; ++i) upper[i] = 1; Vecnf<4> aa(std::vector({1,2})); std::cout << aa << std::endl; SamplerR<4> sampler(lower, upper); for(std::size_t i = 0; i < 10; ++i) std::cout << sampler.sample() << std::endl; // Disabled broken test lines. Please see #25. // SamplerSE2 sampler2(0, 1, -1, 1); // for(std::size_t i = 0; i < 10; ++i) // std::cout << sampler2.sample() << std::endl; SamplerSE3Euler sampler3(Vec3f(0, 0, 0), Vec3f(1, 1, 1)); for(std::size_t i = 0; i < 10; ++i) std::cout << sampler3.sample() << std::endl; } BOOST_AUTO_TEST_CASE(projection_test_line) { Vec3f v1(0, 0, 0); Vec3f v2(2, 0, 0); Vec3f p(1, 0, 0); Project::ProjectResult res = Project::projectLine(v1, v2, p); BOOST_CHECK(res.encode == 3); BOOST_CHECK(approx(res.sqr_distance, 0)); BOOST_CHECK(approx(res.parameterization[0], 0.5)); BOOST_CHECK(approx(res.parameterization[1], 0.5)); p = Vec3f(-1, 0, 0); res = Project::projectLine(v1, v2, p); BOOST_CHECK(res.encode == 1); BOOST_CHECK(approx(res.sqr_distance, 1)); BOOST_CHECK(approx(res.parameterization[0], 1)); BOOST_CHECK(approx(res.parameterization[1], 0)); p = Vec3f(3, 0, 0); res = Project::projectLine(v1, v2, p); BOOST_CHECK(res.encode == 2); BOOST_CHECK(approx(res.sqr_distance, 1)); BOOST_CHECK(approx(res.parameterization[0], 0)); BOOST_CHECK(approx(res.parameterization[1], 1)); } BOOST_AUTO_TEST_CASE(projection_test_triangle) { Vec3f v1(0, 0, 1); Vec3f v2(0, 1, 0); Vec3f v3(1, 0, 0); Vec3f p(1, 1, 1); Project::ProjectResult res = Project::projectTriangle(v1, v2, v3, p); BOOST_CHECK(res.encode == 7); BOOST_CHECK(approx(res.sqr_distance, 4/3.0)); BOOST_CHECK(approx(res.parameterization[0], 1/3.0)); BOOST_CHECK(approx(res.parameterization[1], 1/3.0)); BOOST_CHECK(approx(res.parameterization[2], 1/3.0)); p = Vec3f(0, 0, 1.5); res = Project::projectTriangle(v1, v2, v3, p); BOOST_CHECK(res.encode == 1); BOOST_CHECK(approx(res.sqr_distance, 0.25)); BOOST_CHECK(approx(res.parameterization[0], 1)); BOOST_CHECK(approx(res.parameterization[1], 0)); BOOST_CHECK(approx(res.parameterization[2], 0)); p = Vec3f(1.5, 0, 0); res = Project::projectTriangle(v1, v2, v3, p); BOOST_CHECK(res.encode == 4); BOOST_CHECK(approx(res.sqr_distance, 0.25)); BOOST_CHECK(approx(res.parameterization[0], 0)); BOOST_CHECK(approx(res.parameterization[1], 0)); BOOST_CHECK(approx(res.parameterization[2], 1)); p = Vec3f(0, 1.5, 0); res = Project::projectTriangle(v1, v2, v3, p); BOOST_CHECK(res.encode == 2); BOOST_CHECK(approx(res.sqr_distance, 0.25)); BOOST_CHECK(approx(res.parameterization[0], 0)); BOOST_CHECK(approx(res.parameterization[1], 1)); BOOST_CHECK(approx(res.parameterization[2], 0)); p = Vec3f(1, 1, 0); res = Project::projectTriangle(v1, v2, v3, p); BOOST_CHECK(res.encode == 6); BOOST_CHECK(approx(res.sqr_distance, 0.5)); BOOST_CHECK(approx(res.parameterization[0], 0)); BOOST_CHECK(approx(res.parameterization[1], 0.5)); BOOST_CHECK(approx(res.parameterization[2], 0.5)); p = Vec3f(1, 0, 1); res = Project::projectTriangle(v1, v2, v3, p); BOOST_CHECK(res.encode == 5); BOOST_CHECK(approx(res.sqr_distance, 0.5)); BOOST_CHECK(approx(res.parameterization[0], 0.5)); BOOST_CHECK(approx(res.parameterization[1], 0)); BOOST_CHECK(approx(res.parameterization[2], 0.5)); p = Vec3f(0, 1, 1); res = Project::projectTriangle(v1, v2, v3, p); BOOST_CHECK(res.encode == 3); BOOST_CHECK(approx(res.sqr_distance, 0.5)); BOOST_CHECK(approx(res.parameterization[0], 0.5)); BOOST_CHECK(approx(res.parameterization[1], 0.5)); BOOST_CHECK(approx(res.parameterization[2], 0)); } BOOST_AUTO_TEST_CASE(projection_test_tetrahedron) { Vec3f v1(0, 0, 1); Vec3f v2(0, 1, 0); Vec3f v3(1, 0, 0); Vec3f v4(1, 1, 1); Vec3f p(0.5, 0.5, 0.5); Project::ProjectResult res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 15); BOOST_CHECK(approx(res.sqr_distance, 0)); BOOST_CHECK(approx(res.parameterization[0], 0.25)); BOOST_CHECK(approx(res.parameterization[1], 0.25)); BOOST_CHECK(approx(res.parameterization[2], 0.25)); BOOST_CHECK(approx(res.parameterization[3], 0.25)); p = Vec3f(0, 0, 0); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 7); BOOST_CHECK(approx(res.sqr_distance, 1/3.0)); BOOST_CHECK(approx(res.parameterization[0], 1/3.0)); BOOST_CHECK(approx(res.parameterization[1], 1/3.0)); BOOST_CHECK(approx(res.parameterization[2], 1/3.0)); BOOST_CHECK(approx(res.parameterization[3], 0)); p = Vec3f(0, 1, 1); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 11); BOOST_CHECK(approx(res.sqr_distance, 1/3.0)); BOOST_CHECK(approx(res.parameterization[0], 1/3.0)); BOOST_CHECK(approx(res.parameterization[1], 1/3.0)); BOOST_CHECK(approx(res.parameterization[2], 0)); BOOST_CHECK(approx(res.parameterization[3], 1/3.0)); p = Vec3f(1, 1, 0); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 14); BOOST_CHECK(approx(res.sqr_distance, 1/3.0)); BOOST_CHECK(approx(res.parameterization[0], 0)); BOOST_CHECK(approx(res.parameterization[1], 1/3.0)); BOOST_CHECK(approx(res.parameterization[2], 1/3.0)); BOOST_CHECK(approx(res.parameterization[3], 1/3.0)); p = Vec3f(1, 0, 1); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 13); BOOST_CHECK(approx(res.sqr_distance, 1/3.0)); BOOST_CHECK(approx(res.parameterization[0], 1/3.0)); BOOST_CHECK(approx(res.parameterization[1], 0)); BOOST_CHECK(approx(res.parameterization[2], 1/3.0)); BOOST_CHECK(approx(res.parameterization[3], 1/3.0)); p = Vec3f(1.5, 1.5, 1.5); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 8); BOOST_CHECK(approx(res.sqr_distance, 0.75)); BOOST_CHECK(approx(res.parameterization[0], 0)); BOOST_CHECK(approx(res.parameterization[1], 0)); BOOST_CHECK(approx(res.parameterization[2], 0)); BOOST_CHECK(approx(res.parameterization[3], 1)); p = Vec3f(1.5, -0.5, -0.5); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 4); BOOST_CHECK(approx(res.sqr_distance, 0.75)); BOOST_CHECK(approx(res.parameterization[0], 0)); BOOST_CHECK(approx(res.parameterization[1], 0)); BOOST_CHECK(approx(res.parameterization[2], 1)); BOOST_CHECK(approx(res.parameterization[3], 0)); p = Vec3f(-0.5, -0.5, 1.5); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 1); BOOST_CHECK(approx(res.sqr_distance, 0.75)); BOOST_CHECK(approx(res.parameterization[0], 1)); BOOST_CHECK(approx(res.parameterization[1], 0)); BOOST_CHECK(approx(res.parameterization[2], 0)); BOOST_CHECK(approx(res.parameterization[3], 0)); p = Vec3f(-0.5, 1.5, -0.5); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 2); BOOST_CHECK(approx(res.sqr_distance, 0.75)); BOOST_CHECK(approx(res.parameterization[0], 0)); BOOST_CHECK(approx(res.parameterization[1], 1)); BOOST_CHECK(approx(res.parameterization[2], 0)); BOOST_CHECK(approx(res.parameterization[3], 0)); p = Vec3f(0.5, -0.5, 0.5); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 5); BOOST_CHECK(approx(res.sqr_distance, 0.25)); BOOST_CHECK(approx(res.parameterization[0], 0.5)); BOOST_CHECK(approx(res.parameterization[1], 0)); BOOST_CHECK(approx(res.parameterization[2], 0.5)); BOOST_CHECK(approx(res.parameterization[3], 0)); p = Vec3f(0.5, 1.5, 0.5); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 10); BOOST_CHECK(approx(res.sqr_distance, 0.25)); BOOST_CHECK(approx(res.parameterization[0], 0)); BOOST_CHECK(approx(res.parameterization[1], 0.5)); BOOST_CHECK(approx(res.parameterization[2], 0)); BOOST_CHECK(approx(res.parameterization[3], 0.5)); p = Vec3f(1.5, 0.5, 0.5); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 12); BOOST_CHECK(approx(res.sqr_distance, 0.25)); BOOST_CHECK(approx(res.parameterization[0], 0)); BOOST_CHECK(approx(res.parameterization[1], 0)); BOOST_CHECK(approx(res.parameterization[2], 0.5)); BOOST_CHECK(approx(res.parameterization[3], 0.5)); p = Vec3f(-0.5, 0.5, 0.5); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 3); BOOST_CHECK(approx(res.sqr_distance, 0.25)); BOOST_CHECK(approx(res.parameterization[0], 0.5)); BOOST_CHECK(approx(res.parameterization[1], 0.5)); BOOST_CHECK(approx(res.parameterization[2], 0)); BOOST_CHECK(approx(res.parameterization[3], 0)); p = Vec3f(0.5, 0.5, 1.5); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 9); BOOST_CHECK(approx(res.sqr_distance, 0.25)); BOOST_CHECK(approx(res.parameterization[0], 0.5)); BOOST_CHECK(approx(res.parameterization[1], 0)); BOOST_CHECK(approx(res.parameterization[2], 0)); BOOST_CHECK(approx(res.parameterization[3], 0.5)); p = Vec3f(0.5, 0.5, -0.5); res = Project::projectTetrahedra(v1, v2, v3, v4, p); BOOST_CHECK(res.encode == 6); BOOST_CHECK(approx(res.sqr_distance, 0.25)); BOOST_CHECK(approx(res.parameterization[0], 0)); BOOST_CHECK(approx(res.parameterization[1], 0.5)); BOOST_CHECK(approx(res.parameterization[2], 0.5)); BOOST_CHECK(approx(res.parameterization[3], 0)); } fcl-0.5.0/test/test_fcl_sphere_capsule.cpp000066400000000000000000000143031274356571000206430ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Martin Felis */ #define BOOST_TEST_MODULE "FCL_SPHERE_CAPSULE" #include #include "fcl/math/constants.h" #include "fcl/collision.h" #include "fcl/shape/geometric_shapes.h" #include "fcl/narrowphase/narrowphase.h" using namespace fcl; BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_separated_z) { GJKSolver_libccd solver; Sphere sphere1 (50); Transform3f sphere1_transform; sphere1_transform.setTranslation (Vec3f (0., 0., -50)); Capsule capsule (50, 200.); Transform3f capsule_transform (Vec3f (0., 0., 200)); BOOST_CHECK (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, NULL)); } BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_separated_z_negative) { GJKSolver_libccd solver; Sphere sphere1 (50); Transform3f sphere1_transform; sphere1_transform.setTranslation (Vec3f (0., 0., 50)); Capsule capsule (50, 200.); Transform3f capsule_transform (Vec3f (0., 0., -200)); BOOST_CHECK (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, NULL)); } BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_separated_x) { GJKSolver_libccd solver; Sphere sphere1 (50); Transform3f sphere1_transform; sphere1_transform.setTranslation (Vec3f (0., 0., -50)); Capsule capsule (50, 200.); Transform3f capsule_transform (Vec3f (150., 0., 0.)); BOOST_CHECK (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, NULL)); } BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_separated_capsule_rotated) { GJKSolver_libccd solver; Sphere sphere1 (50); Transform3f sphere1_transform; sphere1_transform.setTranslation (Vec3f (0., 0., -50)); Capsule capsule (50, 200.); Matrix3f rotation; rotation.setEulerZYX (constants::pi * 0.5, 0., 0.); Transform3f capsule_transform (rotation, Vec3f (150., 0., 0.)); BOOST_CHECK (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, NULL)); } BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_penetration_z) { GJKSolver_libccd solver; Sphere sphere1 (50); Transform3f sphere1_transform; sphere1_transform.setTranslation (Vec3f (0., 0., -50)); Capsule capsule (50, 200.); Transform3f capsule_transform (Vec3f (0., 0., 125)); std::vector contacts; bool is_intersecting = solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, &contacts); FCL_REAL penetration = contacts[0].penetration_depth; Vec3f contact_point = contacts[0].pos; Vec3f normal = contacts[0].normal; BOOST_CHECK (is_intersecting); BOOST_CHECK (penetration == 25.); BOOST_CHECK (Vec3f (0., 0., 1.).equal(normal)); BOOST_CHECK (Vec3f (0., 0., 0.).equal(contact_point)); } BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_penetration_z_rotated) { GJKSolver_libccd solver; Sphere sphere1 (50); Transform3f sphere1_transform; sphere1_transform.setTranslation (Vec3f (0., 0., 0)); Capsule capsule (50, 200.); Matrix3f rotation; rotation.setEulerZYX (constants::pi * 0.5, 0., 0.); Transform3f capsule_transform (rotation, Vec3f (0., 50., 75)); std::vector contacts; bool is_intersecting = solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, &contacts); FCL_REAL penetration = contacts[0].penetration_depth; Vec3f contact_point = contacts[0].pos; Vec3f normal = contacts[0].normal; BOOST_CHECK (is_intersecting); BOOST_CHECK_CLOSE (25, penetration, solver.collision_tolerance); BOOST_CHECK (Vec3f (0., 0., 1.).equal(normal)); BOOST_CHECK (Vec3f (0., 0., 50.).equal(contact_point, solver.collision_tolerance)); } BOOST_AUTO_TEST_CASE(Sphere_Capsule_Distance_test_collision) { GJKSolver_libccd solver; Sphere sphere1 (50); Transform3f sphere1_transform; sphere1_transform.setTranslation (Vec3f (0., 0., -50)); Capsule capsule (50, 200.); Transform3f capsule_transform (Vec3f (0., 0., 100)); FCL_REAL distance; BOOST_CHECK (!solver.shapeDistance(sphere1, sphere1_transform, capsule, capsule_transform, &distance)); } BOOST_AUTO_TEST_CASE(Sphere_Capsule_Distance_test_separated) { GJKSolver_libccd solver; Sphere sphere1 (50); Transform3f sphere1_transform; sphere1_transform.setTranslation (Vec3f (0., 0., -50)); Capsule capsule (50, 200.); Transform3f capsule_transform (Vec3f (0., 0., 175)); FCL_REAL distance = 0.; Vec3f p1; Vec3f p2; bool is_separated = solver.shapeDistance(sphere1, sphere1_transform, capsule, capsule_transform, &distance); BOOST_CHECK (is_separated); BOOST_CHECK (distance == 25.); } fcl-0.5.0/test/test_fcl_utility.cpp000066400000000000000000000275561274356571000173620ustar00rootroot00000000000000#include "test_fcl_utility.h" #include "fcl/collision.h" #include "fcl/continuous_collision.h" #include "fcl/distance.h" #include #include #include namespace fcl { Timer::Timer() { #ifdef _WIN32 QueryPerformanceFrequency(&frequency); startCount.QuadPart = 0; endCount.QuadPart = 0; #else startCount.tv_sec = startCount.tv_usec = 0; endCount.tv_sec = endCount.tv_usec = 0; #endif stopped = 0; startTimeInMicroSec = 0; endTimeInMicroSec = 0; } Timer::~Timer() { } void Timer::start() { stopped = 0; // reset stop flag #ifdef _WIN32 QueryPerformanceCounter(&startCount); #else gettimeofday(&startCount, NULL); #endif } void Timer::stop() { stopped = 1; // set timer stopped flag #ifdef _WIN32 QueryPerformanceCounter(&endCount); #else gettimeofday(&endCount, NULL); #endif } double Timer::getElapsedTimeInMicroSec() { #ifdef _WIN32 if(!stopped) QueryPerformanceCounter(&endCount); startTimeInMicroSec = startCount.QuadPart * (1000000.0 / frequency.QuadPart); endTimeInMicroSec = endCount.QuadPart * (1000000.0 / frequency.QuadPart); #else if(!stopped) gettimeofday(&endCount, NULL); startTimeInMicroSec = (startCount.tv_sec * 1000000.0) + startCount.tv_usec; endTimeInMicroSec = (endCount.tv_sec * 1000000.0) + endCount.tv_usec; #endif return endTimeInMicroSec - startTimeInMicroSec; } double Timer::getElapsedTimeInMilliSec() { return this->getElapsedTimeInMicroSec() * 0.001; } double Timer::getElapsedTimeInSec() { return this->getElapsedTimeInMicroSec() * 0.000001; } double Timer::getElapsedTime() { return this->getElapsedTimeInMilliSec(); } FCL_REAL rand_interval(FCL_REAL rmin, FCL_REAL rmax) { FCL_REAL t = rand() / ((FCL_REAL)RAND_MAX + 1); return (t * (rmax - rmin) + rmin); } void loadOBJFile(const char* filename, std::vector& points, std::vector& triangles) { FILE* file = fopen(filename, "rb"); if(!file) { std::cerr << "file not exist" << std::endl; return; } bool has_normal = false; bool has_texture = false; char line_buffer[2000]; while(fgets(line_buffer, 2000, file)) { char* first_token = strtok(line_buffer, "\r\n\t "); if(!first_token || first_token[0] == '#' || first_token[0] == 0) continue; switch(first_token[0]) { case 'v': { if(first_token[1] == 'n') { strtok(NULL, "\t "); strtok(NULL, "\t "); strtok(NULL, "\t "); has_normal = true; } else if(first_token[1] == 't') { strtok(NULL, "\t "); strtok(NULL, "\t "); has_texture = true; } else { FCL_REAL x = (FCL_REAL)atof(strtok(NULL, "\t ")); FCL_REAL y = (FCL_REAL)atof(strtok(NULL, "\t ")); FCL_REAL z = (FCL_REAL)atof(strtok(NULL, "\t ")); Vec3f p(x, y, z); points.push_back(p); } } break; case 'f': { Triangle tri; char* data[30]; int n = 0; while((data[n] = strtok(NULL, "\t \r\n")) != NULL) { if(strlen(data[n])) n++; } for(int t = 0; t < (n - 2); ++t) { if((!has_texture) && (!has_normal)) { tri[0] = atoi(data[0]) - 1; tri[1] = atoi(data[1]) - 1; tri[2] = atoi(data[2]) - 1; } else { const char *v1; for(int i = 0; i < 3; i++) { // vertex ID if(i == 0) v1 = data[0]; else v1 = data[t + i]; tri[i] = atoi(v1) - 1; } } triangles.push_back(tri); } } } } } void saveOBJFile(const char* filename, std::vector& points, std::vector& triangles) { std::ofstream os(filename); if(!os) { std::cerr << "file not exist" << std::endl; return; } for(std::size_t i = 0; i < points.size(); ++i) { os << "v " << points[i][0] << " " << points[i][1] << " " << points[i][2] << std::endl; } for(std::size_t i = 0; i < triangles.size(); ++i) { os << "f " << triangles[i][0] + 1 << " " << triangles[i][1] + 1 << " " << triangles[i][2] + 1 << std::endl; } os.close(); } void eulerToMatrix(FCL_REAL a, FCL_REAL b, FCL_REAL c, Matrix3f& R) { FCL_REAL c1 = cos(a); FCL_REAL c2 = cos(b); FCL_REAL c3 = cos(c); FCL_REAL s1 = sin(a); FCL_REAL s2 = sin(b); FCL_REAL s3 = sin(c); R.setValue(c1 * c2, - c2 * s1, s2, c3 * s1 + c1 * s2 * s3, c1 * c3 - s1 * s2 * s3, - c2 * s3, s1 * s3 - c1 * c3 * s2, c3 * s1 * s2 + c1 * s3, c2 * c3); } void generateRandomTransform(FCL_REAL extents[6], Transform3f& transform) { FCL_REAL x = rand_interval(extents[0], extents[3]); FCL_REAL y = rand_interval(extents[1], extents[4]); FCL_REAL z = rand_interval(extents[2], extents[5]); const FCL_REAL pi = 3.1415926; FCL_REAL a = rand_interval(0, 2 * pi); FCL_REAL b = rand_interval(0, 2 * pi); FCL_REAL c = rand_interval(0, 2 * pi); Matrix3f R; eulerToMatrix(a, b, c, R); Vec3f T(x, y, z); transform.setTransform(R, T); } void generateRandomTransforms(FCL_REAL extents[6], std::vector& transforms, std::size_t n) { transforms.resize(n); for(std::size_t i = 0; i < n; ++i) { FCL_REAL x = rand_interval(extents[0], extents[3]); FCL_REAL y = rand_interval(extents[1], extents[4]); FCL_REAL z = rand_interval(extents[2], extents[5]); const FCL_REAL pi = 3.1415926; FCL_REAL a = rand_interval(0, 2 * pi); FCL_REAL b = rand_interval(0, 2 * pi); FCL_REAL c = rand_interval(0, 2 * pi); { Matrix3f R; eulerToMatrix(a, b, c, R); Vec3f T(x, y, z); transforms[i].setTransform(R, T); } } } void generateRandomTransforms(FCL_REAL extents[6], FCL_REAL delta_trans[3], FCL_REAL delta_rot, std::vector& transforms, std::vector& transforms2, std::size_t n) { transforms.resize(n); transforms2.resize(n); for(std::size_t i = 0; i < n; ++i) { FCL_REAL x = rand_interval(extents[0], extents[3]); FCL_REAL y = rand_interval(extents[1], extents[4]); FCL_REAL z = rand_interval(extents[2], extents[5]); const FCL_REAL pi = 3.1415926; FCL_REAL a = rand_interval(0, 2 * pi); FCL_REAL b = rand_interval(0, 2 * pi); FCL_REAL c = rand_interval(0, 2 * pi); { Matrix3f R; eulerToMatrix(a, b, c, R); Vec3f T(x, y, z); transforms[i].setTransform(R, T); } FCL_REAL deltax = rand_interval(-delta_trans[0], delta_trans[0]); FCL_REAL deltay = rand_interval(-delta_trans[1], delta_trans[1]); FCL_REAL deltaz = rand_interval(-delta_trans[2], delta_trans[2]); FCL_REAL deltaa = rand_interval(-delta_rot, delta_rot); FCL_REAL deltab = rand_interval(-delta_rot, delta_rot); FCL_REAL deltac = rand_interval(-delta_rot, delta_rot); { Matrix3f R; eulerToMatrix(a + deltaa, b + deltab, c + deltac, R); Vec3f T(x + deltax, y + deltay, z + deltaz); transforms2[i].setTransform(R, T); } } } void generateRandomTransform_ccd(FCL_REAL extents[6], std::vector& transforms, std::vector& transforms2, FCL_REAL delta_trans[3], FCL_REAL delta_rot, std::size_t n, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2) { transforms.resize(n); transforms2.resize(n); for(std::size_t i = 0; i < n;) { FCL_REAL x = rand_interval(extents[0], extents[3]); FCL_REAL y = rand_interval(extents[1], extents[4]); FCL_REAL z = rand_interval(extents[2], extents[5]); const FCL_REAL pi = 3.1415926; FCL_REAL a = rand_interval(0, 2 * pi); FCL_REAL b = rand_interval(0, 2 * pi); FCL_REAL c = rand_interval(0, 2 * pi); Matrix3f R; eulerToMatrix(a, b, c, R); Vec3f T(x, y, z); Transform3f tf(R, T); std::vector > results; { transforms[i] = tf; FCL_REAL deltax = rand_interval(-delta_trans[0], delta_trans[0]); FCL_REAL deltay = rand_interval(-delta_trans[1], delta_trans[1]); FCL_REAL deltaz = rand_interval(-delta_trans[2], delta_trans[2]); FCL_REAL deltaa = rand_interval(-delta_rot, delta_rot); FCL_REAL deltab = rand_interval(-delta_rot, delta_rot); FCL_REAL deltac = rand_interval(-delta_rot, delta_rot); Matrix3f R2; eulerToMatrix(a + deltaa, b + deltab, c + deltac, R2); Vec3f T2(x + deltax, y + deltay, z + deltaz); transforms2[i].setTransform(R2, T2); ++i; } } } bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cdata_) { CollisionData* cdata = static_cast(cdata_); const CollisionRequest& request = cdata->request; CollisionResult& result = cdata->result; if(cdata->done) return true; collide(o1, o2, request, result); if(!request.enable_cost && (result.isCollision()) && (result.numContacts() >= request.num_max_contacts)) cdata->done = true; return cdata->done; } bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* cdata_, FCL_REAL& dist) { DistanceData* cdata = static_cast(cdata_); const DistanceRequest& request = cdata->request; DistanceResult& result = cdata->result; if(cdata->done) { dist = result.min_distance; return true; } distance(o1, o2, request, result); dist = result.min_distance; if(dist <= 0) return true; // in collision or in touch return cdata->done; } bool defaultContinuousCollisionFunction(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata_) { ContinuousCollisionData* cdata = static_cast(cdata_); const ContinuousCollisionRequest& request = cdata->request; ContinuousCollisionResult& result = cdata->result; if(cdata->done) return true; collide(o1, o2, request, result); return cdata->done; } bool defaultContinuousDistanceFunction(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata_, FCL_REAL& dist) { return true; } std::string getNodeTypeName(NODE_TYPE node_type) { if (node_type == BV_UNKNOWN) return std::string("BV_UNKNOWN"); else if (node_type == BV_AABB) return std::string("BV_AABB"); else if (node_type == BV_OBB) return std::string("BV_OBB"); else if (node_type == BV_RSS) return std::string("BV_RSS"); else if (node_type == BV_kIOS) return std::string("BV_kIOS"); else if (node_type == BV_OBBRSS) return std::string("BV_OBBRSS"); else if (node_type == BV_KDOP16) return std::string("BV_KDOP16"); else if (node_type == BV_KDOP18) return std::string("BV_KDOP18"); else if (node_type == BV_KDOP24) return std::string("BV_KDOP24"); else if (node_type == GEOM_BOX) return std::string("GEOM_BOX"); else if (node_type == GEOM_SPHERE) return std::string("GEOM_SPHERE"); else if (node_type == GEOM_ELLIPSOID) return std::string("GEOM_ELLIPSOID"); else if (node_type == GEOM_CAPSULE) return std::string("GEOM_CAPSULE"); else if (node_type == GEOM_CONE) return std::string("GEOM_CONE"); else if (node_type == GEOM_CYLINDER) return std::string("GEOM_CYLINDER"); else if (node_type == GEOM_CONVEX) return std::string("GEOM_CONVEX"); else if (node_type == GEOM_PLANE) return std::string("GEOM_PLANE"); else if (node_type == GEOM_HALFSPACE) return std::string("GEOM_HALFSPACE"); else if (node_type == GEOM_TRIANGLE) return std::string("GEOM_TRIANGLE"); else if (node_type == GEOM_OCTREE) return std::string("GEOM_OCTREE"); else return std::string("invalid"); } std::string getGJKSolverName(GJKSolverType solver_type) { if (solver_type == GST_LIBCCD) return std::string("libccd"); else if (solver_type == GST_INDEP) return std::string("built-in"); else return std::string("invalid"); } } fcl-0.5.0/test/test_fcl_utility.h000066400000000000000000000157201274356571000170150ustar00rootroot00000000000000/* * Software License Agreement (BSD License) * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2016, Open Source Robotics Foundation * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of Open Source Robotics Foundation 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 OWNER 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. */ /** \author Jia Pan */ #ifndef TEST_FCL_UTILITY_H #define TEST_FCL_UTILITY_H #include "fcl/math/transform.h" #include "fcl/collision_data.h" #include "fcl/collision_object.h" #ifdef _WIN32 #define NOMINMAX // required to avoid compilation errors with Visual Studio 2010 #include #else #include #endif namespace fcl { class Timer { public: Timer(); ~Timer(); void start(); ///< start timer void stop(); ///< stop the timer double getElapsedTime(); ///< get elapsed time in milli-second double getElapsedTimeInSec(); ///< get elapsed time in second (same as getElapsedTime) double getElapsedTimeInMilliSec(); ///< get elapsed time in milli-second double getElapsedTimeInMicroSec(); ///< get elapsed time in micro-second private: double startTimeInMicroSec; ///< starting time in micro-second double endTimeInMicroSec; ///< ending time in micro-second int stopped; ///< stop flag #ifdef _WIN32 LARGE_INTEGER frequency; ///< ticks per second LARGE_INTEGER startCount; LARGE_INTEGER endCount; #else timeval startCount; timeval endCount; #endif }; /// @brief Load an obj mesh file void loadOBJFile(const char* filename, std::vector& points, std::vector& triangles); void saveOBJFile(const char* filename, std::vector& points, std::vector& triangles); /// @brief Generate one random transform whose translation is constrained by extents and rotation without constraints. /// The translation is (x, y, z), and extents[0] <= x <= extents[3], extents[1] <= y <= extents[4], extents[2] <= z <= extents[5] void generateRandomTransform(FCL_REAL extents[6], Transform3f& transform); /// @brief Generate n random transforms whose translations are constrained by extents. void generateRandomTransforms(FCL_REAL extents[6], std::vector& transforms, std::size_t n); /// @brief Generate n random transforms whose translations are constrained by extents. Also generate another transforms2 which have additional random translation & rotation to the transforms generated. void generateRandomTransforms(FCL_REAL extents[6], FCL_REAL delta_trans[3], FCL_REAL delta_rot, std::vector& transforms, std::vector& transforms2, std::size_t n); /// @brief Generate n random tranforms and transform2 with addtional random translation/rotation. The transforms and transform2 are used as initial and goal configurations for the first mesh. The second mesh is in I. This is used for continuous collision detection checking. void generateRandomTransforms_ccd(FCL_REAL extents[6], std::vector& transforms, std::vector& transforms2, FCL_REAL delta_trans[3], FCL_REAL delta_rot, std::size_t n, const std::vector& vertices1, const std::vector& triangles1, const std::vector& vertices2, const std::vector& triangles2); /// @ brief Structure for minimum distance between two meshes and the corresponding nearest point pair struct DistanceRes { double distance; Vec3f p1; Vec3f p2; }; /// @brief Collision data stores the collision request and the result given by collision algorithm. struct CollisionData { CollisionData() { done = false; } /// @brief Collision request CollisionRequest request; /// @brief Collision result CollisionResult result; /// @brief Whether the collision iteration can stop bool done; }; /// @brief Distance data stores the distance request and the result given by distance algorithm. struct DistanceData { DistanceData() { done = false; } /// @brief Distance request DistanceRequest request; /// @brief Distance result DistanceResult result; /// @brief Whether the distance iteration can stop bool done; }; /// @brief Continuous collision data stores the continuous collision request and result given the continuous collision algorithm. struct ContinuousCollisionData { ContinuousCollisionData() { done = false; } /// @brief Continuous collision request ContinuousCollisionRequest request; /// @brief Continuous collision result ContinuousCollisionResult result; /// @brief Whether the continuous collision iteration can stop bool done; }; /// @brief Default collision callback for two objects o1 and o2 in broad phase. return value means whether the broad phase can stop now. bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cdata); /// @brief Default distance callback for two objects o1 and o2 in broad phase. return value means whether the broad phase can stop now. also return dist, i.e. the bmin distance till now bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& dist); bool defaultContinuousCollisionFunction(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata_); bool defaultContinuousDistanceFunction(ContinuousCollisionObject* o1, ContinuousCollisionObject* o2, void* cdata_, FCL_REAL& dist); std::string getNodeTypeName(NODE_TYPE node_type); std::string getGJKSolverName(GJKSolverType solver_type); } #endif